<commit_before>#include "inertial-measurement-unit.h"

#include "constants.h"

namespace rcr {
namespace level1payload {

InertialMeasurementUnit::InertialMeasurementUnit()
  : Sensor(KALMAN_PROCESS_NOISE, KALMAN_MEASUREMENT_NOISE, KALMAN_ERROR) {
  heading_ = kalmanInit(0);
  roll_    = kalmanInit(0);
  pitch_   = kalmanInit(0);

  // Linear accelleration:
  Lx_     = kalmanInit(0);;
  Ly_     = kalmanInit(0);;
  Lg_     = kalmanInit(0);;

  // Gravitational accelleration:
  Gx_     = kalmanInit(0);;
  Gy_     = kalmanInit(0);;
  Gz_     = kalmanInit(0);;
}

bool InertialMeasurementUnit::Init() {
  return bno_.begin();
}

// TODO: Is possible overflow an issue?
template <int SampleSize>
imu::Vector<3> InertialMeasurementUnit::SampleForMeanVector (
    Adafruit_BNO055::adafruit_vector_type_t vector_type) {
  // Resultant vector of arithmetic-mean x,y,z values.
  auto mean_vector = imu::Vector<3>();

  // Ensure x,y,z == 0.0
  mean_vector.x() = 0.0;
  mean_vector.y() = 0.0;
  mean_vector.z() = 0.0;

  // Sample the IMU sensor.
  auto imu_vectors = std::vector<imu::Vector<3>>(static_cast<size_t>(SampleSize));
  for (auto i = 0; i < SampleSize; ++i) {
    imu_vectors.push_back(
      std::move(bno_.getVector(vector_type))
    );
  }

  // Compute the mean.
  auto sample_size_double = static_cast<double>(SampleSize);
  for (auto& v : imu_vectors) {
    //Serial.println(v.x());
    mean_vector.x() += v.x();
    mean_vector.y() += v.y();
    mean_vector.z() += v.z();
  }
  mean_vector.x() /= sample_size_double;
  mean_vector.y() /= sample_size_double;
  mean_vector.z() /= sample_size_double;

  //Serial.println(mean_vector.x());
  return mean_vector;
}

void InertialMeasurementUnit::GetCsvLine(String* string_to_append) {
  //  Sample the IMU.
  auto orientation = SampleForMeanVector<2>(Adafruit_BNO055::VECTOR_EULER);
  auto linear = SampleForMeanVector<2>(Adafruit_BNO055::VECTOR_LINEARACCEL);
  auto gravity = SampleForMeanVector<2>(Adafruit_BNO055::VECTOR_GRAVITY);

  // Update respective Kalman filters.
    // (1) Euler vector:
  kalmanUpdate(&euler_x_, orientation.x);
  kalmanUpdate(&euler_y_, orientation.y);
  kalmanUpdate(&euler_z_, orientation.z);

    // (2) Linear accelleration vector:
  kalmanUpdate(&linear_x_, linear.x);
  kalmanUpdate(&linear_y_, linear.y);
  kalmanUpdate(&linear_z_, linear.z);

    // (3) Gravitational accelleration vector:
  kalmanUpdate(&gravity_x_, gravity.x);
  kalmanUpdate(&gravity_y_, gravity.y);
  kalmanUpdate(&gravity_z_, gravity.z);

  // Return filtered results.
  *string_to_append += euler_x_.value;
  *string_to_append += ",";
  *string_to_append += euler_y_.value;
  *string_to_append += ",";
  *string_to_append += euler_z_.value;
  *string_to_append += ",";

  *string_to_append += linear_x_.value;
  *string_to_append += ",";
  *string_to_append += linear_y_.value;
  *string_to_append += ",";
  *string_to_append += linear_z_.value;<commit_msg>make casts explicit<commit_after>#include "inertial-measurement-unit.h"

#include "constants.h"

#include <vector>
#include <utility>

namespace rcr {
namespace level1payload {

InertialMeasurementUnit::InertialMeasurementUnit()
  : Sensor(KALMAN_PROCESS_NOISE, KALMAN_MEASUREMENT_NOISE, KALMAN_ERROR) {
  euler_x_   = kalmanInit(0);
  euler_y_   = kalmanInit(0);
  euler_z_   = kalmanInit(0);

  // Linear accelleration:
  linear_x_  = kalmanInit(0);;
  linear_y_  = kalmanInit(0);;
  linear_z_  = kalmanInit(0);;

  // Gravitational accelleration:
  gravity_x_ = kalmanInit(0);;
  gravity_y_ = kalmanInit(0);;
  gravity_z_ = kalmanInit(0);;
}

bool InertialMeasurementUnit::Init() {
  return bno_.begin();
}

// TODO: Is possible overflow an issue?
template <int SampleSize>
imu::Vector<3> InertialMeasurementUnit::SampleForMeanVector (
    Adafruit_BNO055::adafruit_vector_type_t vector_type) {
  // Resultant vector of arithmetic-mean x,y,z values.
  auto mean_vector = imu::Vector<3>();

  // Ensure x,y,z == 0.0
  mean_vector.x() = 0.0;
  mean_vector.y() = 0.0;
  mean_vector.z() = 0.0;

  // Sample the IMU sensor.
  auto imu_vectors = std::vector<imu::Vector<3>>(static_cast<size_t>(SampleSize));
  for (auto i = 0; i < SampleSize; ++i) {
    imu_vectors.push_back(
      std::move(bno_.getVector(vector_type))
    );
  }

  // Compute the mean.
  auto sample_size_double = static_cast<double>(SampleSize);
  for (auto& v : imu_vectors) {
    //Serial.println(v.x());
    mean_vector.x() += v.x();
    mean_vector.y() += v.y();
    mean_vector.z() += v.z();
  }
  mean_vector.x() /= sample_size_double;
  mean_vector.y() /= sample_size_double;
  mean_vector.z() /= sample_size_double;

  //Serial.println(mean_vector.x());
  return mean_vector;
}

void InertialMeasurementUnit::GetCsvLine(String* string_to_append) {
  //  Sample the IMU.
  auto orientation = SampleForMeanVector<2>(Adafruit_BNO055::VECTOR_EULER);
  auto linear = SampleForMeanVector<2>(Adafruit_BNO055::VECTOR_LINEARACCEL);
  auto gravity = SampleForMeanVector<2>(Adafruit_BNO055::VECTOR_GRAVITY);

  // Update respective Kalman filters.
    // (1) Euler vector:
  kalmanUpdate(&euler_x_, float{ orientation.x() });
  kalmanUpdate(&euler_y_, float{ orientation.y() });
  kalmanUpdate(&euler_z_, float{ orientation.z() });

    // (2) Linear accelleration vector:
  kalmanUpdate(&linear_x_, float{ linear.x() });
  kalmanUpdate(&linear_y_, float{ linear.y() });
  kalmanUpdate(&linear_z_, float{ linear.z() });

    // (3) Gravitational accelleration vector:
  kalmanUpdate(&gravity_x_, float{ gravity.x() });
  kalmanUpdate(&gravity_y_, float{ gravity.y() });
  kalmanUpdate(&gravity_z_, float{ gravity.z() });

  // Return filtered results.
  *string_to_append += euler_x_.value;
  *string_to_append += ",";
  *string_to_append += euler_y_.value;
  *string_to_append += ",";
  *string_to_append += euler_z_.value;
  *string_to_append += ",";

  *string_to_append += linear_x_.value;
  *string_to_append += ",";
  *string_to_append += linear_y_.value;
  *string_to_append += ",";
  *string_to_append += linear_z_.value;
{'fraction_non_alphanumeric': 0.1507554374896231, 'fraction_numerical': 0.013282417399966794, 'mean_word_length': 4.526605504587156, 'pattern_counts': {'":': 0, '<': 27, '<?xml version=': 0, '>': 25, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2966984', 'n_tokens_mistral': 2382, 'n_tokens_neox': 2269, 'n_words': 372}