bno08x.ipp

Go to the documentation of this file.

 
#ifndef BNO085_HEADER_INCLUDED
#include "bno08x.hpp"
#endif
 
#include <algorithm>
#include "dfu/MemoryFirmware.hpp"
 
// ============================================================================
// ============================================================================
 
template <typename CommType>
void BNO085<CommType>::prepareHalWrapper() noexcept {
  halWrapper_.comm = &io_;
  halWrapper_.hal.open = halOpen;
  halWrapper_.hal.close = halClose;
  halWrapper_.hal.read = halRead;
  halWrapper_.hal.write = halWrite;
  halWrapper_.hal.getTimeUs = halGetTimeUs;
}
 
template <typename CommType>
bool BNO085<CommType>::Begin() noexcept {
  if (state_ == BNO085DriverState::Sh2Active)
    return true;
  if (state_ != BNO085DriverState::Closed) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return false;
  }
  if (io_.GetInterfaceType() == BNO085Interface::UARTRVC) {
    last_error_ = SH2_ERR_BAD_PARAM;
    return false;
  }
 
  last_error_ = 0;
  prepareHalWrapper();
 
  int status = sh2_open(halWrapper_.asHal(), asyncCallback, this);
  if (status != SH2_OK) {
    last_error_ = status;
    io_.Close();
    return false;
  }
  status = sh2_reinitialize();
  if (status != SH2_OK) {
    last_error_ = status;
    sh2_close();
    io_.Close();
    return false;
  }
  status = sh2_setSensorCallback(sensorCallback, this);
  if (status != SH2_OK) {
    last_error_ = status;
    sh2_close();
    io_.Close();
    return false;
  }
 
  new_flag_.fill(false);
  last_interval_.fill(0);
  last_sensitivity_.fill(0);
  state_ = BNO085DriverState::Sh2Active;
  return true;
}
 
template <typename CommType>
bool BNO085<CommType>::EnableSensor(BNO085Sensor sensor, uint32_t interval_ms,
                                    float sensitivity) noexcept {
  if (state_ != BNO085DriverState::Sh2Active) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return false;
  }
  auto id = static_cast<uint8_t>(sensor);
  if (id >= last_interval_.size())
    return false;
  uint32_t interval_us = interval_ms * 1000;
  if (!configure(sensor, interval_us, sensitivity, 0))
    return false;
  last_interval_[id] = interval_us;
  last_sensitivity_[id] = sensitivity;
  return true;
}
 
template <typename CommType>
bool BNO085<CommType>::DisableSensor(BNO085Sensor sensor) noexcept {
  if (state_ != BNO085DriverState::Sh2Active) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return false;
  }
  auto id = static_cast<uint8_t>(sensor);
  if (id >= last_interval_.size())
    return false;
  if (!configure(sensor, 0, 0, 0))
    return false;
  last_interval_[id] = 0;
  last_sensitivity_[id] = 0.0f;
  new_flag_[id] = false;
  return true;
}
 
template <typename CommType>
void BNO085<CommType>::SetCallback(SensorCallback cb) noexcept {
  callback_ = cb;
}
 
template <typename CommType>
void BNO085<CommType>::SetRvcCallback(RvcCallback cb) noexcept {
  rvc_cb_ = cb;
}
 
template <typename CommType>
bool BNO085<CommType>::HasNewData(BNO085Sensor sensor) const {
  if (state_ != BNO085DriverState::Sh2Active)
    return false;
  auto id = static_cast<uint8_t>(sensor);
  if (id >= new_flag_.size())
    return false;
  return new_flag_[id];
}
 
template <typename CommType>
SensorEvent BNO085<CommType>::toSensorEvent(BNO085Sensor sensor,
                                            const sh2_SensorValue_t& val) const noexcept {
  SensorEvent out{};
  out.sensor = sensor;
  out.timestamp = val.timestamp;
  out.detected = false;
  const uint8_t accuracy = val.status & 0x03;
 
  switch (sensor) {
  case BNO085Sensor::RawAccelerometer:
    out.raw.x = val.un.rawAccelerometer.x;
    out.raw.y = val.un.rawAccelerometer.y;
    out.raw.z = val.un.rawAccelerometer.z;
    out.raw.sensorTimeUs = val.un.rawAccelerometer.timestamp;
    break;
  case BNO085Sensor::Accelerometer:
    out.vector.x = val.un.accelerometer.x;
    out.vector.y = val.un.accelerometer.y;
    out.vector.z = val.un.accelerometer.z;
    out.vector.accuracy = accuracy;
    break;
  case BNO085Sensor::LinearAcceleration:
    out.vector.x = val.un.linearAcceleration.x;
    out.vector.y = val.un.linearAcceleration.y;
    out.vector.z = val.un.linearAcceleration.z;
    out.vector.accuracy = accuracy;
    break;
  case BNO085Sensor::Gravity:
    out.vector.x = val.un.gravity.x;
    out.vector.y = val.un.gravity.y;
    out.vector.z = val.un.gravity.z;
    out.vector.accuracy = accuracy;
    break;
  case BNO085Sensor::RawGyroscope:
    out.raw.x = val.un.rawGyroscope.x;
    out.raw.y = val.un.rawGyroscope.y;
    out.raw.z = val.un.rawGyroscope.z;
    out.raw.temperature = val.un.rawGyroscope.temperature;
    out.raw.sensorTimeUs = val.un.rawGyroscope.timestamp;
    break;
  case BNO085Sensor::Gyroscope:
    out.vector.x = val.un.gyroscope.x;
    out.vector.y = val.un.gyroscope.y;
    out.vector.z = val.un.gyroscope.z;
    out.vector.accuracy = accuracy;
    break;
  case BNO085Sensor::GyroUncalibrated:
    out.vector.x = val.un.gyroscopeUncal.x;
    out.vector.y = val.un.gyroscopeUncal.y;
    out.vector.z = val.un.gyroscopeUncal.z;
    out.vector.accuracy = accuracy;
    out.bias.x = val.un.gyroscopeUncal.biasX;
    out.bias.y = val.un.gyroscopeUncal.biasY;
    out.bias.z = val.un.gyroscopeUncal.biasZ;
    break;
  case BNO085Sensor::RawMagnetometer:
    out.raw.x = val.un.rawMagnetometer.x;
    out.raw.y = val.un.rawMagnetometer.y;
    out.raw.z = val.un.rawMagnetometer.z;
    out.raw.sensorTimeUs = val.un.rawMagnetometer.timestamp;
    break;
  case BNO085Sensor::Magnetometer:
    out.vector.x = val.un.magneticField.x;
    out.vector.y = val.un.magneticField.y;
    out.vector.z = val.un.magneticField.z;
    out.vector.accuracy = accuracy;
    break;
  case BNO085Sensor::MagneticFieldUncalibrated:
    out.vector.x = val.un.magneticFieldUncal.x;
    out.vector.y = val.un.magneticFieldUncal.y;
    out.vector.z = val.un.magneticFieldUncal.z;
    out.vector.accuracy = accuracy;
    out.bias.x = val.un.magneticFieldUncal.biasX;
    out.bias.y = val.un.magneticFieldUncal.biasY;
    out.bias.z = val.un.magneticFieldUncal.biasZ;
    break;
  case BNO085Sensor::RotationVector:
    out.rotation.w = val.un.rotationVector.real;
    out.rotation.x = val.un.rotationVector.i;
    out.rotation.y = val.un.rotationVector.j;
    out.rotation.z = val.un.rotationVector.k;
    out.rotation.accuracy = accuracy;
    break;
  case BNO085Sensor::GameRotationVector:
    out.rotation.w = val.un.gameRotationVector.real;
    out.rotation.x = val.un.gameRotationVector.i;
    out.rotation.y = val.un.gameRotationVector.j;
    out.rotation.z = val.un.gameRotationVector.k;
    out.rotation.accuracy = accuracy;
    break;
  case BNO085Sensor::GeomagneticRotationVector:
    out.rotation.w = val.un.geoMagRotationVector.real;
    out.rotation.x = val.un.geoMagRotationVector.i;
    out.rotation.y = val.un.geoMagRotationVector.j;
    out.rotation.z = val.un.geoMagRotationVector.k;
    out.rotation.accuracy = accuracy;
    break;
  case BNO085Sensor::ARVRStabilizedRV:
    out.rotation.w = val.un.arvrStabilizedRV.real;
    out.rotation.x = val.un.arvrStabilizedRV.i;
    out.rotation.y = val.un.arvrStabilizedRV.j;
    out.rotation.z = val.un.arvrStabilizedRV.k;
    out.rotation.accuracy = accuracy;
    break;
  case BNO085Sensor::ARVRStabilizedGameRV:
    out.rotation.w = val.un.arvrStabilizedGRV.real;
    out.rotation.x = val.un.arvrStabilizedGRV.i;
    out.rotation.y = val.un.arvrStabilizedGRV.j;
    out.rotation.z = val.un.arvrStabilizedGRV.k;
    out.rotation.accuracy = accuracy;
    break;
  case BNO085Sensor::GyroIntegratedRV:
    out.rotation.w = val.un.gyroIntegratedRV.real;
    out.rotation.x = val.un.gyroIntegratedRV.i;
    out.rotation.y = val.un.gyroIntegratedRV.j;
    out.rotation.z = val.un.gyroIntegratedRV.k;
    out.rotation.accuracy = accuracy;
    out.angularVelocity.x = val.un.gyroIntegratedRV.angVelX;
    out.angularVelocity.y = val.un.gyroIntegratedRV.angVelY;
    out.angularVelocity.z = val.un.gyroIntegratedRV.angVelZ;
    break;
  case BNO085Sensor::Pressure:
    out.scalar = val.un.pressure.value;
    break;
  case BNO085Sensor::AmbientLight:
    out.scalar = val.un.ambientLight.value;
    break;
  case BNO085Sensor::Humidity:
    out.scalar = val.un.humidity.value;
    break;
  case BNO085Sensor::Proximity:
    out.scalar = val.un.proximity.value;
    break;
  case BNO085Sensor::Temperature:
    out.scalar = val.un.temperature.value;
    break;
  case BNO085Sensor::StepDetector:
    out.latencyUs = val.un.stepDetector.latency;
    out.detected = true;
    out.eventFlags = 1;
    break;
  case BNO085Sensor::StepCounter:
    out.latencyUs = val.un.stepCounter.latency;
    out.stepCount = val.un.stepCounter.steps;
    break;
  case BNO085Sensor::SignificantMotion:
    out.eventFlags = val.un.sigMotion.motion;
    out.detected = out.eventFlags != 0;
    break;
  case BNO085Sensor::StabilityClassifier:
    out.classification = val.un.stabilityClassifier.classification;
    break;
  case BNO085Sensor::TapDetector:
    out.eventFlags = val.un.tapDetector.flags;
    out.tap.doubleTap = (val.un.tapDetector.flags & TAPDET_DOUBLE);
    if (val.un.tapDetector.flags & TAPDET_X)
      out.tap.direction = (val.un.tapDetector.flags & TAPDET_X_POS) ? 0 : 1;
    else if (val.un.tapDetector.flags & TAPDET_Y)
      out.tap.direction = (val.un.tapDetector.flags & TAPDET_Y_POS) ? 2 : 3;
    else if (val.un.tapDetector.flags & TAPDET_Z)
      out.tap.direction = (val.un.tapDetector.flags & TAPDET_Z_POS) ? 4 : 5;
    else
      out.tap.direction = 0;
    out.detected = val.un.tapDetector.flags & (TAPDET_X | TAPDET_Y | TAPDET_Z);
    break;
  case BNO085Sensor::ShakeDetector:
    out.eventFlags = val.un.shakeDetector.shake;
    out.detected = out.eventFlags != 0;
    break;
  case BNO085Sensor::FlipDetector:
    out.eventFlags = val.un.flipDetector.flip;
    out.detected = out.eventFlags != 0;
    break;
  case BNO085Sensor::PickupDetector:
    out.eventFlags = val.un.pickupDetector.pickup;
    out.detected = out.eventFlags != 0;
    break;
  case BNO085Sensor::StabilityDetector:
    out.eventFlags = val.un.stabilityDetector.stability;
    out.detected = out.eventFlags != 0;
    break;
  case BNO085Sensor::PersonalActivityClassifier:
    out.activity.page = val.un.personalActivityClassifier.page;
    out.activity.lastPage = val.un.personalActivityClassifier.lastPage;
    out.activity.mostLikelyState = val.un.personalActivityClassifier.mostLikelyState;
    for (std::size_t i = 0; i < out.activity.confidence.size(); ++i) {
      out.activity.confidence[i] = val.un.personalActivityClassifier.confidence[i];
    }
    out.classification = out.activity.mostLikelyState;
    break;
  case BNO085Sensor::SleepDetector:
    out.sleepState = val.un.sleepDetector.sleepState;
    out.detected = out.sleepState != 0;
    break;
  case BNO085Sensor::TiltDetector:
    out.eventFlags = val.un.tiltDetector.tilt;
    out.detected = out.eventFlags != 0;
    break;
  case BNO085Sensor::PocketDetector:
    out.eventFlags = val.un.pocketDetector.pocket;
    out.detected = out.eventFlags != 0;
    break;
  case BNO085Sensor::CircleDetector:
    out.eventFlags = val.un.circleDetector.circle;
    out.detected = out.eventFlags != 0;
    break;
  case BNO085Sensor::HeartRateMonitor:
    out.scalar = static_cast<float>(val.un.heartRateMonitor.heartRate);
    break;
  case BNO085Sensor::IZroMotionRequest:
    out.motionIntent = static_cast<uint8_t>(val.un.izroRequest.intent);
    out.motionRequest = static_cast<uint8_t>(val.un.izroRequest.request);
    break;
  case BNO085Sensor::RawOpticalFlow:
    out.rawOpticalFlow.timestamp = val.un.rawOptFlow.timestamp;
    out.rawOpticalFlow.dt = val.un.rawOptFlow.dt;
    out.rawOpticalFlow.dx = val.un.rawOptFlow.dx;
    out.rawOpticalFlow.dy = val.un.rawOptFlow.dy;
    out.rawOpticalFlow.iq = val.un.rawOptFlow.iq;
    out.rawOpticalFlow.resX = val.un.rawOptFlow.resX;
    out.rawOpticalFlow.resY = val.un.rawOptFlow.resY;
    out.rawOpticalFlow.shutter = val.un.rawOptFlow.shutter;
    out.rawOpticalFlow.frameMax = val.un.rawOptFlow.frameMax;
    out.rawOpticalFlow.frameAvg = val.un.rawOptFlow.frameAvg;
    out.rawOpticalFlow.frameMin = val.un.rawOptFlow.frameMin;
    out.rawOpticalFlow.laserOn = val.un.rawOptFlow.laserOn;
    break;
  case BNO085Sensor::DeadReckoningPose:
    out.deadReckoningPose.timestamp = val.un.deadReckoningPose.timestamp;
    out.deadReckoningPose.linearPosition.x = val.un.deadReckoningPose.linPosX;
    out.deadReckoningPose.linearPosition.y = val.un.deadReckoningPose.linPosY;
    out.deadReckoningPose.linearPosition.z = val.un.deadReckoningPose.linPosZ;
    out.deadReckoningPose.rotation.w = val.un.deadReckoningPose.real;
    out.deadReckoningPose.rotation.x = val.un.deadReckoningPose.i;
    out.deadReckoningPose.rotation.y = val.un.deadReckoningPose.j;
    out.deadReckoningPose.rotation.z = val.un.deadReckoningPose.k;
    out.deadReckoningPose.linearVelocity.x = val.un.deadReckoningPose.linVelX;
    out.deadReckoningPose.linearVelocity.y = val.un.deadReckoningPose.linVelY;
    out.deadReckoningPose.linearVelocity.z = val.un.deadReckoningPose.linVelZ;
    out.deadReckoningPose.angularVelocity.x = val.un.deadReckoningPose.angVelX;
    out.deadReckoningPose.angularVelocity.y = val.un.deadReckoningPose.angVelY;
    out.deadReckoningPose.angularVelocity.z = val.un.deadReckoningPose.angVelZ;
    break;
  case BNO085Sensor::WheelEncoder:
    out.wheelEncoder.timestamp = val.un.wheelEncoder.timestamp;
    out.wheelEncoder.wheelIndex = val.un.wheelEncoder.wheelIndex;
    out.wheelEncoder.dataType = val.un.wheelEncoder.dataType;
    out.wheelEncoder.data = val.un.wheelEncoder.data;
    break;
  default:
    break;
  }
  return out;
}
 
template <typename CommType>
SensorEvent BNO085<CommType>::GetLatest(BNO085Sensor sensor) noexcept {
  SensorEvent out{};
  out.sensor = sensor;
  if (state_ != BNO085DriverState::Sh2Active)
    return out;
  const auto id = static_cast<uint8_t>(sensor);
  if (id >= latest_.size())
    return out;
  out = toSensorEvent(sensor, latest_[id]);
  new_flag_[id] = false;
  return out;
}
 
template <typename CommType>
void BNO085<CommType>::Update() noexcept {
  if (state_ == BNO085DriverState::Sh2Active) {
    sh2_service();
  }
}
 
template <typename CommType>
void BNO085<CommType>::Close() noexcept {
  if (state_ == BNO085DriverState::DfuInProgress) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return;
  }
  if (state_ == BNO085DriverState::RvcActive) {
    io_.Close();
    state_ = BNO085DriverState::Closed;
    rvc_frame_len_ = 0;
    return;
  }
  if (state_ != BNO085DriverState::Sh2Active)
    return;
  sh2_close();
  io_.Close();
  state_ = BNO085DriverState::Closed;
  new_flag_.fill(false);
  last_interval_.fill(0);
  last_sensitivity_.fill(0.0f);
}
 
// ---- SH-2 HAL callback bridges (CRTP CommInterface -> C sh2_Hal_t) ---------
 
template <typename CommType>
int BNO085<CommType>::halOpen(sh2_Hal_t* self) {
  auto* t = reinterpret_cast<CommHal*>(self);
  return t->comm->Open() ? SH2_OK : SH2_ERR;
}
 
template <typename CommType>
void BNO085<CommType>::halClose(sh2_Hal_t* self) {
  reinterpret_cast<CommHal*>(self)->comm->Close();
}
 
template <typename CommType>
int BNO085<CommType>::halRead(sh2_Hal_t* self, uint8_t* buf, unsigned len, uint32_t* t) {
  auto* th = reinterpret_cast<CommHal*>(self);
  int ret = th->comm->Read(buf, len);
  *t = th->comm->GetTimeUs();
  return ret;
}
 
template <typename CommType>
int BNO085<CommType>::halWrite(sh2_Hal_t* self, uint8_t* buf, unsigned len) {
  return reinterpret_cast<CommHal*>(self)->comm->Write(buf, len);
}
 
template <typename CommType>
uint32_t BNO085<CommType>::halGetTimeUs(sh2_Hal_t* self) {
  return reinterpret_cast<CommHal*>(self)->comm->GetTimeUs();
}
 
// ---- SH-2 event callback trampolines --------------------------------------
 
template <typename CommType>
void BNO085<CommType>::sensorCallback(void* cookie, sh2_SensorEvent_t* event) {
  static_cast<BNO085<CommType>*>(cookie)->handleSensorEvent(event);
}
 
template <typename CommType>
void BNO085<CommType>::asyncCallback(void* cookie, sh2_AsyncEvent_t* event) {
  static_cast<BNO085<CommType>*>(cookie)->handleAsyncEvent(event);
}
 
template <typename CommType>
void BNO085<CommType>::handleSensorEvent(const sh2_SensorEvent_t* event) noexcept {
  sh2_SensorValue_t value;
  int decode_status = sh2_decodeSensorEvent(&value, event);
  if (decode_status != SH2_OK) {
    last_error_ = decode_status;
    return;
  }
  uint8_t id = value.sensorId;
  if (id >= latest_.size())
    return;
  latest_[id] = value;
  new_flag_[id] = true;
  if (callback_) {
    callback_(toSensorEvent(static_cast<BNO085Sensor>(id), value));
  }
}
 
template <typename CommType>
void BNO085<CommType>::handleAsyncEvent(const sh2_AsyncEvent_t* event) noexcept {
  if (event->eventId == SH2_RESET) {
    for (uint8_t id = 0; id < last_interval_.size(); ++id) {
      if (last_interval_[id])
        configure(static_cast<BNO085Sensor>(id), last_interval_[id], last_sensitivity_[id], 0);
    }
  }
}
 
template <typename CommType>
bool BNO085<CommType>::configure(BNO085Sensor sensor, uint32_t interval_us, float sensitivity,
                                 uint32_t batch_us) noexcept {
  sh2_SensorConfig_t cfg{};
  cfg.reportInterval_us = interval_us;
  cfg.batchInterval_us = batch_us;
  cfg.sensorSpecific = 0;
  const float clamped_sensitivity = std::clamp(sensitivity, 0.0f, 65535.0f);
  cfg.changeSensitivity = static_cast<uint16_t>(clamped_sensitivity + 0.5f);
  cfg.changeSensitivityEnabled = clamped_sensitivity > 0.0f;
  int status = sh2_setSensorConfig(static_cast<sh2_SensorId_t>(sensor), &cfg);
  if (status != SH2_OK) {
    last_error_ = status;
    return false;
  }
  return true;
}
 
// ============================================================================
// ============================================================================
 
template <typename CommType>
void BNO085<CommType>::HardwareReset(uint32_t lowMs) noexcept {
  using bno08x::CtrlPin;
  io_.GpioSetActive(CtrlPin::RSTN);   // Assert reset
  io_.Delay(lowMs);
  io_.GpioSetInactive(CtrlPin::RSTN); // Release reset
  io_.Delay(50);                      // Wait for sensor boot
}
 
template <typename CommType>
void BNO085<CommType>::SetBootPin(bool state) noexcept {
  using bno08x::CtrlPin;
  using bno08x::GpioSignal;
  io_.GpioSet(CtrlPin::BOOTN, state ? GpioSignal::ACTIVE : GpioSignal::INACTIVE);
}
 
template <typename CommType>
bool BNO085<CommType>::EnterBootloader(uint32_t resetLowMs, uint32_t settleMs) noexcept {
  if (io_.GetInterfaceType() == BNO085Interface::UARTRVC) {
    last_error_ = SH2_ERR_BAD_PARAM;
    return false;
  }
  if (state_ == BNO085DriverState::DfuInProgress) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return false;
  }
  if (state_ != BNO085DriverState::Closed) {
    Close();
  }
  if (state_ != BNO085DriverState::Closed) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return false;
  }
 
  io_.GpioSetActive(bno08x::CtrlPin::BOOTN);   // Assert BOOTN (enter bootloader)
  HardwareReset(resetLowMs);
  io_.GpioSetInactive(bno08x::CtrlPin::BOOTN); // Release BOOTN after reset
  if (settleMs) {
    io_.Delay(settleMs);
  }
  last_error_ = SH2_OK;
  return true;
}
 
template <typename CommType>
bool BNO085<CommType>::ExitBootloaderAndReboot(uint32_t resetLowMs, uint32_t settleMs) noexcept {
  if (io_.GetInterfaceType() == BNO085Interface::UARTRVC) {
    last_error_ = SH2_ERR_BAD_PARAM;
    return false;
  }
  if (state_ == BNO085DriverState::DfuInProgress) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return false;
  }
  if (state_ != BNO085DriverState::Closed) {
    Close();
  }
  if (state_ != BNO085DriverState::Closed) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return false;
  }
 
  io_.GpioSetInactive(bno08x::CtrlPin::BOOTN); // Ensure normal application boot path
  HardwareReset(resetLowMs);
  if (settleMs) {
    io_.Delay(settleMs);
  }
  last_error_ = SH2_OK;
  return true;
}
 
template <typename CommType>
void BNO085<CommType>::SetWakePin(bool state) noexcept {
  using bno08x::CtrlPin;
  using bno08x::GpioSignal;
  io_.GpioSet(CtrlPin::WAKE, state ? GpioSignal::ACTIVE : GpioSignal::INACTIVE);
}
 
template <typename CommType>
void BNO085<CommType>::SelectInterface(BNO085Interface iface) noexcept {
  using bno08x::CtrlPin;
  using bno08x::GpioSignal;
  switch (iface) {
  case BNO085Interface::I2C:
    io_.GpioSetInactive(CtrlPin::PS1);
    io_.GpioSetInactive(CtrlPin::PS0);
    break;
  case BNO085Interface::UARTRVC:
    io_.GpioSetActive(CtrlPin::PS1);
    io_.GpioSetInactive(CtrlPin::PS0);
    break;
  case BNO085Interface::UART:
    io_.GpioSetInactive(CtrlPin::PS1);
    io_.GpioSetActive(CtrlPin::PS0);
    break;
  case BNO085Interface::SPI:
    io_.GpioSetActive(CtrlPin::PS1);
    io_.GpioSetActive(CtrlPin::PS0);
    break;
  }
}
 
// ============================================================================
// ============================================================================
 
template <typename CommType>
bool BNO085<CommType>::BeginRvc() noexcept {
  if (state_ == BNO085DriverState::RvcActive)
    return true;
  if (state_ != BNO085DriverState::Closed) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return false;
  }
  if (io_.GetInterfaceType() != BNO085Interface::UARTRVC) {
    last_error_ = SH2_ERR_BAD_PARAM;
    return false;
  }
  if (!io_.Open()) {
    last_error_ = SH2_ERR;
    return false;
  }
  last_error_ = SH2_OK;
  rvc_frame_len_ = 0;
  state_ = BNO085DriverState::RvcActive;
  return true;
}
 
template <typename CommType>
void BNO085<CommType>::ServiceRvc() noexcept {
  if (state_ != BNO085DriverState::RvcActive)
    return;
  uint8_t c;
  while (io_.Read(&c, 1) == 1) {
    rvcProcessByte(c);
  }
}
 
template <typename CommType>
void BNO085<CommType>::CloseRvc() noexcept {
  if (state_ == BNO085DriverState::RvcActive) {
    io_.Close();
    state_ = BNO085DriverState::Closed;
    rvc_frame_len_ = 0;
  }
}
 
template <typename CommType>
void BNO085<CommType>::rvcProcessByte(uint8_t c) noexcept {
  if (rvc_frame_len_ == RVC_FRAME_LEN_) {
    std::memmove(rvc_frame_, rvc_frame_ + 1, RVC_FRAME_LEN_ - 1);
    rvc_frame_[RVC_FRAME_LEN_ - 1] = c;
  } else {
    rvc_frame_[rvc_frame_len_++] = c;
  }
 
  if (rvc_frame_len_ == RVC_FRAME_LEN_ && rvc_frame_[0] == 0xAA && rvc_frame_[1] == 0xAA) {
    uint8_t check = 0;
    for (int i = 2; i < RVC_FRAME_LEN_ - 1; ++i)
      check += rvc_frame_[i];
 
    if (check == rvc_frame_[RVC_FRAME_LEN_ - 1]) {
      RvcSensorEvent event{};
      event.timestamp_uS = io_.GetTimeUs();
      event.index = rvc_frame_[2];
      event.yaw = static_cast<int16_t>((rvc_frame_[4] << 8) | rvc_frame_[3]);
      event.pitch = static_cast<int16_t>((rvc_frame_[6] << 8) | rvc_frame_[5]);
      event.roll = static_cast<int16_t>((rvc_frame_[8] << 8) | rvc_frame_[7]);
      event.acc_x = static_cast<int16_t>((rvc_frame_[10] << 8) | rvc_frame_[9]);
      event.acc_y = static_cast<int16_t>((rvc_frame_[12] << 8) | rvc_frame_[11]);
      event.acc_z = static_cast<int16_t>((rvc_frame_[14] << 8) | rvc_frame_[13]);
      event.mi = rvc_frame_[15];
      event.mr = rvc_frame_[16];
 
      if (rvc_cb_) {
        RvcSensorValue val;
        decodeRvc(&val, &event);
        rvc_cb_(val);
      }
      rvc_frame_len_ = 0;
    }
  }
}
 
template <typename CommType>
void BNO085<CommType>::decodeRvc(RvcSensorValue* out, const RvcSensorEvent* in) noexcept {
  out->index = in->index;
  out->yaw_deg = 0.01f * in->yaw;
  out->pitch_deg = 0.01f * in->pitch;
  out->roll_deg = 0.01f * in->roll;
  out->acc_x_g = 0.001f * in->acc_x;
  out->acc_y_g = 0.001f * in->acc_y;
  out->acc_z_g = 0.001f * in->acc_z;
  out->mi = in->mi;
  out->mr = in->mr;
  out->timestamp_uS = in->timestamp_uS;
}
 
// ============================================================================
// ============================================================================
 
static constexpr uint8_t DFU_ACK = 's';                 
static constexpr uint32_t DFU_MAX_PACKET_LEN = 64;      
static constexpr uint32_t DFU_MAX_ATTEMPTS = 5;         
static constexpr uint32_t DFU_DELAY_POST_US = 10000;    
static constexpr uint32_t DFU_SEND_TIMEOUT_US = 100000; 
 
template <typename CommType>
bool BNO085<CommType>::dfuIsKnownPartNumber(const char* part) noexcept {
  if (!part)
    return false;
  return std::strcmp(part, "1000-3608") == 0 || std::strcmp(part, "1000-3676") == 0 ||
         std::strcmp(part, "1000-4148") == 0 || std::strcmp(part, "1000-4563") == 0;
}
 
template <typename CommType>
void BNO085<CommType>::dfuWrite32be(uint8_t* buf, uint32_t value) noexcept {
  *buf++ = (value >> 24) & 0xFF;
  *buf++ = (value >> 16) & 0xFF;
  *buf++ = (value >> 8) & 0xFF;
  *buf++ = (value >> 0) & 0xFF;
}
 
template <typename CommType>
void BNO085<CommType>::dfuAppendCrc(uint8_t* packet, uint8_t len) noexcept {
  uint16_t crc = 0xFFFF;
  for (int n = 0; n < len; n++) {
    uint16_t x = static_cast<uint16_t>(packet[n]) << 8;
    for (int i = 0; i < 8; i++) {
      if ((crc ^ x) & 0x8000)
        crc = (crc << 1) ^ 0x1021;
      else
        crc = crc << 1;
      x <<= 1;
    }
  }
  packet[len] = (crc >> 8) & 0xFF;
  packet[len + 1] = crc & 0xFF;
}
 
template <typename CommType>
int BNO085<CommType>::dfuSend(uint8_t* dfu_buff, uint8_t* p_data, uint32_t len) noexcept {
  unsigned int retries = 0;
  int status = SH2_OK;
  uint8_t ack = 0;
  bool got_ack = false;
  uint32_t t;
  sh2_Hal_t* hal = halWrapper_.asHal();
 
  while (!got_ack && (retries < DFU_MAX_ATTEMPTS)) {
    uint32_t now = hal->getTimeUs(hal);
    uint32_t start = now;
    status = 0;
    while ((status == 0) && ((now - start) < DFU_SEND_TIMEOUT_US)) {
      status = hal->write(hal, p_data, len);
      now = hal->getTimeUs(hal);
    }
    if (status == 0)
      status = SH2_ERR_TIMEOUT;
    if (status > 0) {
      status = 0;
      while ((status == 0) && ((now - start) < DFU_SEND_TIMEOUT_US)) {
        status = hal->read(hal, &ack, 1, &t);
        now = hal->getTimeUs(hal);
      }
      if (status == 0)
        status = SH2_ERR_TIMEOUT;
    }
    if (status > 0) {
      if (ack == DFU_ACK) {
        got_ack = true;
        status = SH2_OK;
      } else {
        got_ack = false;
        status = SH2_ERR_HUB;
      }
    }
    if (!got_ack)
      retries++;
  }
  return (status >= 0) ? SH2_OK : status;
}
 
template <typename CommType>
int BNO085<CommType>::dfuSendAppSize(uint8_t* dfu_buff, uint32_t app_size) noexcept {
  dfuWrite32be(dfu_buff, app_size);
  dfuAppendCrc(dfu_buff, 4);
  return dfuSend(dfu_buff, dfu_buff, 6);
}
 
template <typename CommType>
int BNO085<CommType>::dfuSendPktSize(uint8_t* dfu_buff, uint8_t packet_len) noexcept {
  dfu_buff[0] = packet_len;
  dfuAppendCrc(dfu_buff, 1);
  return dfuSend(dfu_buff, dfu_buff, 3);
}
 
template <typename CommType>
int BNO085<CommType>::dfuSendPkt(uint8_t* dfu_buff, uint8_t* p_data, uint32_t len) noexcept {
  std::memcpy(dfu_buff, p_data, len);
  dfuAppendCrc(dfu_buff, len);
  return dfuSend(dfu_buff, dfu_buff, len + 2);
}
 
template <typename CommType>
int BNO085<CommType>::Dfu(const HcBin_t& fw) noexcept {
  DfuOptions options{};
  return DfuWithOptions(fw, options);
}
 
template <typename CommType>
int BNO085<CommType>::DfuFromMemory(const DfuMemoryImage& image,
                                    const DfuOptions& options) noexcept {
  if (!image.data || image.length == 0) {
    last_error_ = SH2_ERR_BAD_PARAM;
    return SH2_ERR_BAD_PARAM;
  }
  MemoryFirmware memfw(image.data, image.length, image.format ? image.format : "BNO_V1",
                       image.partNumber ? image.partNumber : "unknown", image.preferredPacketLen);
  return DfuWithOptions(memfw.hcbin(), options);
}
 
template <typename CommType>
int BNO085<CommType>::DfuFromMemory(const uint8_t* data, uint32_t len, const char* partNumber,
                                    const DfuOptions& options) noexcept {
  DfuMemoryImage image{};
  image.data = data;
  image.length = len;
  image.partNumber = partNumber ? partNumber : "unknown";
  return DfuFromMemory(image, options);
}
 
template <typename CommType>
int BNO085<CommType>::RunDfuFromMemory(const DfuMemoryImage& image, const DfuOptions& options,
                                       uint32_t enterResetLowMs, uint32_t enterSettleMs,
                                       uint32_t exitResetLowMs, uint32_t exitSettleMs) noexcept {
  if (!EnterBootloader(enterResetLowMs, enterSettleMs))
    return last_error_;
 
  int status = DfuFromMemory(image, options);
  const int dfu_status = status;
  if (!ExitBootloaderAndReboot(exitResetLowMs, exitSettleMs) && status == SH2_OK) {
    // Reboot failure becomes the workflow result only if DFU transfer succeeded.
    status = last_error_;
  }
 
  if (dfu_status != SH2_OK) {
    return dfu_status;
  }
  return status;
}
 
template <typename CommType>
int BNO085<CommType>::DfuWithOptions(const HcBin_t& fw, const DfuOptions& options) noexcept {
  if (io_.GetInterfaceType() == BNO085Interface::UARTRVC) {
    last_error_ = SH2_ERR_BAD_PARAM;
    return SH2_ERR_BAD_PARAM;
  }
  if (state_ == BNO085DriverState::RvcActive || state_ == BNO085DriverState::DfuInProgress) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return SH2_ERR_OP_IN_PROGRESS;
  }
  if (state_ == BNO085DriverState::Sh2Active) {
    // DFU uses the same transport HAL and must not race an active SH-2 session.
    Close();
  }
  if (state_ != BNO085DriverState::Closed) {
    last_error_ = SH2_ERR_OP_IN_PROGRESS;
    return SH2_ERR_OP_IN_PROGRESS;
  }
  state_ = BNO085DriverState::DfuInProgress;
  prepareHalWrapper();
 
  int rc, status = SH2_OK;
  uint32_t app_len = 0;
  uint8_t packet_len = 0;
  uint32_t offset = 0;
  const char* meta = nullptr;
  bool fw_opened = false;
  bool hal_opened = false;
  uint8_t dfu_buff[DFU_MAX_PACKET_LEN + 2];
  sh2_Hal_t* hal = halWrapper_.asHal();
 
  rc = fw.open();
  if (rc != 0) {
    status = SH2_ERR;
    goto dfu_end;
  }
  fw_opened = true;
 
  if (options.requireFormatMatch) {
    const char* expected_format = options.requiredFormat ? options.requiredFormat : "BNO_V1";
    meta = fw.getMeta("FW-Format");
    if (!meta || std::strcmp(meta, expected_format) != 0) {
      status = SH2_ERR_BAD_PARAM;
      goto dfu_close;
    }
  }
 
  if (options.requirePartNumber) {
    meta = fw.getMeta("SW-Part-Number");
    if (!meta) {
      status = SH2_ERR_BAD_PARAM;
      goto dfu_close;
    }
    if (options.requiredPartNumber) {
      if (std::strcmp(meta, options.requiredPartNumber) != 0) {
        status = SH2_ERR_BAD_PARAM;
        goto dfu_close;
      }
    } else if (!dfuIsKnownPartNumber(meta)) {
      status = SH2_ERR_BAD_PARAM;
      goto dfu_close;
    }
  }
 
  app_len = fw.getAppLen();
  if (app_len < 1024) {
    status = SH2_ERR_BAD_PARAM;
    goto dfu_close;
  }
 
  packet_len = fw.getPacketLen();
  if (options.packetLenOverride != 0) {
    packet_len = options.packetLenOverride;
  }
  if (packet_len == 0 || packet_len > DFU_MAX_PACKET_LEN)
    packet_len = DFU_MAX_PACKET_LEN;
 
  status = hal->open(hal);
  if (status != SH2_OK)
    goto dfu_close;
  hal_opened = true;
 
  status = dfuSendAppSize(dfu_buff, app_len);
  if (status != SH2_OK)
    goto dfu_close;
  status = dfuSendPktSize(dfu_buff, packet_len);
  if (status != SH2_OK)
    goto dfu_close;
 
  if (options.progress) {
    options.progress(DfuProgress{0, app_len});
  }
 
  offset = 0;
  while (offset < app_len) {
    uint32_t to_send = app_len - offset;
    if (to_send > packet_len)
      to_send = packet_len;
    status = fw.getAppData(dfu_buff, offset, to_send);
    if (status != SH2_OK) {
      status = SH2_ERR;
      goto dfu_close;
    }
    status = dfuSendPkt(dfu_buff, dfu_buff, to_send);
    if (status != SH2_OK)
      goto dfu_close;
    offset += to_send;
    if (options.progress) {
      options.progress(DfuProgress{offset, app_len});
    }
  }
 
dfu_close:
  if (fw_opened)
    fw.close();
  if (status == SH2_OK && hal_opened) {
    uint32_t now = hal->getTimeUs(hal), start = now;
    while ((now - start) < DFU_DELAY_POST_US)
      now = hal->getTimeUs(hal);
  }
  if (hal_opened)
    hal->close(hal);
 
dfu_end:
  state_ = BNO085DriverState::Closed;
  last_error_ = status;
  return status;
}