sensor_fusion_class.cc

Go to the documentation of this file.

/*
 * Copyright (c) 2020-2021, Bjarne Hansen
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
#include "sensor_fusion_class.h"
 
#include <Stream.h>
#include <stdint.h>
 
#include "sensor_fusion/sensor_fusion.h"
#include "sensor_fusion/control.h"
#include "sensor_fusion/driver_sensors.h"
#include "sensor_fusion/status.h"
 
const float kDegToRads = PI / 180.0;   
const float kCelsiusToKelvin = 273.15; 
const float kGeesToMPerSS = 9.80665;   
 
SensorFusion::SensorFusion() {
  sfg_ = new SensorFusionGlobals();
  control_subsystem_ = new ControlSubsystem;
  status_subsystem_ = new StatusSubsystem;
  sensors_ = new PhysicalSensor
      [MAX_NUM_SENSORS];  // this implementation uses up to 4 sensors
                          // (accel/mag; gyro; baro/thermo)
                          // TODO - can use std::vector as per
                          // https://stackoverflow.com/questions/14114686/create-an-array-of-structure-using-new
                          // which will allow adding arbitrary # of sensors.
 
  InitializeInputOutputSubsystem();
  InitializeStatusSubsystem();
  InitializeSensorFusionGlobals();
 
}  // end SensorFusion()
 
bool SensorFusion::InstallSensor(uint8_t sensor_i2c_addr,
                                   SensorType sensor_type) {
 
    if( num_sensors_installed_ >= MAX_NUM_SENSORS ) {
        //already have max number of sensors installed
      return false;
    }
    switch (sensor_type) {
    case SensorType::kAccelerometer:
      sfg_->installSensor(sfg_, &sensors_[num_sensors_installed_],
                          sensor_i2c_addr, kLoopsPerAccelRead, NULL,
                          FXOS8700_Accel_Init, FXOS8700_Accel_Read);
      ++num_sensors_installed_;
      break;
    case SensorType::kMagnetometer:
      sfg_->installSensor(sfg_, &sensors_[num_sensors_installed_],
                          sensor_i2c_addr, kLoopsPerMagRead, NULL,
                          FXOS8700_Mag_Init, FXOS8700_Mag_Read);
      ++num_sensors_installed_;
      break;
    case SensorType::kMagnetometerAccelerometer:
      sfg_->installSensor(sfg_, &sensors_[num_sensors_installed_],
                          sensor_i2c_addr, kLoopsPerAccelRead, NULL,
                          FXOS8700_Init, FXOS8700_Read);
      ++num_sensors_installed_;
      break;
    case SensorType::kGyroscope:
      sfg_->installSensor(sfg_, &sensors_[num_sensors_installed_],
                          sensor_i2c_addr, kLoopsPerGyroRead, NULL,
                          FXAS21002_Init, FXAS21002_Read);
      ++num_sensors_installed_;
      break;
    case SensorType::kThermometer:
      // use the thermometer built into FXOS8700. Not precise nor calibrated,
      // but OK.
      sfg_->installSensor(sfg_, &sensors_[num_sensors_installed_],
                          sensor_i2c_addr, kLoopsPerThermRead, NULL,
                          FXOS8700_Therm_Init, FXOS8700_Therm_Read);
      ++num_sensors_installed_;
      break;
    case SensorType::kBarometer:
      // TODO define some access functions for this
      // TODO add barometer sensor
      break;
    default:
      // unrecognized sensor type
      break;
  }
  return true;
}  // end InstallSensor()
 
bool SensorFusion::InitializeInputOutputSubsystem(const Stream *serial_port,
                                                  const void *tcp_client) {
  return initializeIOSubsystem(control_subsystem_, serial_port, tcp_client);
}  // end InitializeInputOutputSubsystem()
 
void SensorFusion::Begin(int pin_i2c_sda, int pin_i2c_scl) {
  sfg_->initializeFusionEngine(
      sfg_, pin_i2c_sda, pin_i2c_scl);                      // Initialize sensors and magnetic calibration
      //if nothing connected to I2C, won't return.
  sfg_->setStatus(sfg_, NORMAL);  // Set status state to NORMAL
  //try reading sensors, this will update the status to see if initialize worked.
  sfg_->readSensors(sfg_,0);
 
}  // end InitializeFusionEngine()
 
void SensorFusion::UpdateWiFiStream(void *tcp_client) {
  UpdateTCPClient(control_subsystem_, tcp_client);
 
}  // end UpdateTCPClient()
 
void SensorFusion::ReadSensors(void) {
  sfg_->readSensors(
      sfg_,
      loops_per_fuse_counter_);  // Reads sensors, applies HAL, removes -32768
 
}  // end ReadSensors()
 
void SensorFusion::RunFusion(void) {
  // applies fusion algorithm to data accumulated in buffers
 
  // only run fusion every kLoopsPerFusionCalc'th time through loop
  if (loops_per_fuse_counter_ < kLoopsPerFusionCalc) {
    ++loops_per_fuse_counter_;
    return;
  }
 
  sfg_->conditionSensorReadings(sfg_);  // Pre-processing; Magnetic calibration.
  sfg_->runFusion(sfg_);                // Run fusion algorithms
 
  // Serial.printf(" Algo took %ld us\n", sfg.SV_9DOF_GBY_KALMAN.systick);
  sfg_->loopcounter++;  // loop counter is used to "serialize" mag cal
                        // operations and blink LEDs to indicate status
  // LED Blinking is too fast unless status updates are slowed down.
  // Cycle at least four times per status update.
  // TODO - a better way might be to use a timer.
  if (0 == sfg_->loopcounter % 4) {
    sfg_->updateStatus(sfg_);  // make pending status updates visible
  }
 
  // assume NORMAL status next pass through the loop
  // this resets temporary error conditions (SOFT_FAULT)
  sfg_->queueStatus(sfg_, NORMAL);
 
  loops_per_fuse_counter_ = 1;  // reset loop counter
 
}  // end RunFusion()
 
void SensorFusion::ProduceToolboxOutput(void) {
  // Make & send data to Sensor Fusion Toolbox or whatever UART is
  // connected to.
  if (loops_per_fuse_counter_ == 1) {       // only run if fusion has happened
    sfg_->pControlSubsystem->stream(sfg_);  // create output packet
    sfg_->pControlSubsystem->write(sfg_);   // send output packet
  }
 
}  // end ProduceToolboxOutput()
 
bool SensorFusion::SendArbitraryData(const char *buffer, uint16_t data_length) {
  if (data_length > MAX_LEN_SERIAL_OUTPUT_BUF) {
    return false;
  }
  char *out_buf = (char *)(sfg_->pControlSubsystem->serial_out_buf);
  for (uint16_t i = 0; i < data_length; i++) {
    out_buf[i] = buffer[i];
  }
  sfg_->pControlSubsystem->bytes_to_send = data_length;
  sfg_->pControlSubsystem->write(sfg_);  // send output packet
  return true;
}  // end SendArbitraryData()
 
void SensorFusion::ProcessCommands(void) {
  // process any incoming commands
  sfg_->pControlSubsystem->readCommands(sfg_);
}  // end ProcessCommands()
 
void SensorFusion::InjectCommand(const char *command) {
  sfg_->pControlSubsystem->injectCommand(sfg_, (uint8_t *)command, 4);
}  // end InjectCommand()
 
void SensorFusion::SaveMagneticCalibration(void) {
  InjectCommand("SVMC");
}  // end SaveMagneticCalibration()
 
bool SensorFusion::IsDataValid(void) {
  if( NORMAL == sfg_->pStatusSubsystem->status ) {
    return true;
  } else {
    return false;
  }
}  // end IsDataValid()
 
int SensorFusion::GetSystemStatus(void) {
  return (int)sfg_->pStatusSubsystem->status;
}  // end GetSystemStatus()
 
// The following Get____() methods return orientation values
// calculated by the 9DOF Kalman algorithm (the most advanced).
// They have been mapped to match the conventions used for
// vessels:
//  Compass Heading; 0 at magnetic north, and increasing CW.
//  Pitch; 0 with boat level, increasing with Bow Up.
//  Roll; 0 with boat level, increasing with Starboard roll.
//  Turn Rate; positive with turn to Starboard
//  Pitch Rate; positive with Bow moving Up.
//  Roll Rate; positive with increasing Starboard heel.
//  Acceleration; X to Bow, Y to Port, Z Up.
// This works with the Adafruit NXP FXOS8700/FXAS21002
//  breakout board, mounted with X toward the bow, Y to port,
//  and Z (component side of PCB) facing up.
// If the sensor orienatation is different than assumed,
//  you may need to remap the axes below.  If a different
//  sensor board is used, you may need also to change the
//  axes mapping in the file hal_axis_remap.c  The latter
//  mapping is applied *before* the fusion algorithm,
//  whereas the below mapping is applied *after*.
 
float SensorFusion::GetHeadingDegrees(void) {
  // TODO - make generic so it's not dependent on algorithm used
  return (sfg_->SV_9DOF_GBY_KALMAN.fRhoPl <= 90)
             ? (sfg_->SV_9DOF_GBY_KALMAN.fRhoPl + 270.0)
             : (sfg_->SV_9DOF_GBY_KALMAN.fRhoPl - 90.0);
}  // end GetHeadingDegrees()
 
float SensorFusion::GetHeadingRadians(void) {
  return GetHeadingDegrees() * kDegToRads;
}  // end GetHeadingRadians()
 
float SensorFusion::GetPitchDegrees(void) {
  return sfg_->SV_9DOF_GBY_KALMAN.fPhiPl;
}  // end GetPitchDegrees()
 
float SensorFusion::GetPitchRadians(void) {
  return GetPitchDegrees() * kDegToRads;
}  // end GetPitchRadians()
 
float SensorFusion::GetRollDegrees(void) {
  return -(sfg_->SV_9DOF_GBY_KALMAN.fThePl);
}  // end GetRollDegrees()
 
float SensorFusion::GetRollRadians(void) {
  return GetRollDegrees() * kDegToRads;
}  // end GetRollRadians()
 
float SensorFusion::GetTemperatureC(void) {
  return sfg_->Temp.temperatureC;
}  // end GetTemperatureC()
 
float SensorFusion::GetTemperatureK(void) {
  return GetTemperatureC() + kCelsiusToKelvin;
}  // end GetTemperatureK()
 
float SensorFusion::GetTurnRateDegPerS(void) {
  return sfg_->SV_9DOF_GBY_KALMAN.fOmega[2];
}  // end GetTurnRateDegPerS()
 
float SensorFusion::GetTurnRateRadPerS(void) {
  return GetTurnRateDegPerS() * kDegToRads;
}  // end GetTurnRateRadPerS()
 
float SensorFusion::GetPitchRateDegPerS(void) {
  return sfg_->SV_9DOF_GBY_KALMAN.fOmega[0];
}  // end GetPitchRateDegPerS()
 
float SensorFusion::GetPitchRateRadPerS(void) {
  return GetPitchRateDegPerS() * kDegToRads;
}  // end GetPitchRateRadPerS()
 
float SensorFusion::GetRollRateDegPerS(void) {
  return -(sfg_->SV_9DOF_GBY_KALMAN.fOmega[1]);
}  // end GetRollRateDegPerS()
 
float SensorFusion::GetRollRateRadPerS(void) {
  return GetRollRateDegPerS() * kDegToRads;
}  // end GetRollRateRadPerS()
 
float SensorFusion::GetAccelXGees(void) {
  return sfg_->Accel.fGc[1];
}  // end GetAccelXGees()
 
float SensorFusion::GetAccelXMPerSS(void) {
  return GetAccelXGees() * kGeesToMPerSS;
}  // end GetAccelXMPerSS()
 
float SensorFusion::GetAccelYGees(void) {
  return sfg_->Accel.fGc[0];
}  // end GetAccelYGees()
 
float SensorFusion::GetAccelYMPerSS(void) {
  return GetAccelYGees() * kGeesToMPerSS;
}  // end GetAccelYMPerSS()
 
float SensorFusion::GetAccelZGees(void) {
  return sfg_->Accel.fGc[2];
}  // end GetAccelZGees()
 
float SensorFusion::GetAccelZMPerSS(void) {
  return GetAccelZGees() * kGeesToMPerSS;
}  // end GetAccelZMPerSS()
 
void  SensorFusion::GetOrientationQuaternion(Quaternion *quat) {
  quat->q0 = sfg_->SV_9DOF_GBY_KALMAN.fqPl.q0;
  quat->q1 = sfg_->SV_9DOF_GBY_KALMAN.fqPl.q1;
  quat->q2 = sfg_->SV_9DOF_GBY_KALMAN.fqPl.q2;
  quat->q3 = sfg_->SV_9DOF_GBY_KALMAN.fqPl.q3;
}  // end GetOrientationQuaternion()
 
float SensorFusion::GetMagneticFitErrorTrial(void) {
  return sfg_->MagCal.ftrFitErrorpc;
}  // end GetMagneticFitErrorTrial()
 
float SensorFusion::GetMagneticFitError(void) {
  return sfg_->MagCal.fFitErrorpc;
}  // end GetMagneticFitError()
 
float SensorFusion::GetMagneticBMagTrial(void) {
  return sfg_->MagCal.ftrB;
}  // end GetMagneticBMagTrial()
 
float SensorFusion::GetMagneticBMag(void) {
  return sfg_->MagCal.fB;
}  // end GetMagneticBMag()
 
float SensorFusion::GetMagneticInclinationDeg(void) {
  return sfg_->SV_9DOF_GBY_KALMAN.fDeltaPl;
}  // end GetMagneticInclinationDeg()
 
float SensorFusion::GetMagneticInclinationRad(void) {
  return GetMagneticInclinationDeg() * kDegToRads;
}  // end GetMagneticInclinationRad()
 
float SensorFusion::GetMagneticNoiseCovariance(void) {
  return sfg_->SV_9DOF_GBY_KALMAN.fQv6x1[3];
}  // end GetMagneticNoiseCovariance()
 
float SensorFusion::GetMagneticCalSolver(void) {
  return (float)(sfg_->MagCal.iValidMagCal);
}  // end GetMagneticCalSolver()
 
//================= end of Get____() methods ==================
//================= start of private methods ==================
 
void SensorFusion::InitializeStatusSubsystem(void) {
  initializeStatusSubsystem(
      status_subsystem_);  // configure the status sub-system
 
}  // end InitializeStatusSubsystem()
 
void SensorFusion::InitializeSensorFusionGlobals(void) {
  initSensorFusionGlobals(sfg_, status_subsystem_, control_subsystem_);
 
}  // end InitializeSensorFusionGlobals()