TotalStationNoiseSimulator.h

#pragma once
#include <NDEVR/GenericOption.h>
#include <NDEVR/BaseValues.h>
#include <NDEVR/Random.h>
#include <NDEVR/Time.h>
#include <NDEVR/TimeSpan.h>
#include <NDEVR/UnitDefinitions.h>
#include <NDEVR/Angle.h>
#include <NDEVR/AngleDefinitions.h>
#include <NDEVR/Translator.h>
#include "DLLInfo.h"
namespace NDEVR
{
    //Vernier Acuity: Eye reference accuracy,
    //parallax:
    Angle<fltp08> camera_fov;
    //Vector<2, Angle<fltp08>> optical_axis = Vector<2, Angle<fltp08>>();
    //for displacement (e) of encoder ring radius (r) the error is
    Vector<2, fltp08> v_circle_eccentricity = Vector<2, fltp08>(0.0, 1.2e-5);
    Vector<2, fltp08> h_circle_eccentricity = Vector<2, fltp08>(0.0, 1.2e-5);
    fltp08 encoder_radius = 0.005;
    Vector<2, fltp08> optical_axis = Vector<2, fltp08>(0.0, 1.2e-5);
    Vector<2, Angle<fltp08>> optical_collimation = Vector<2, Angle<fltp08>>(
          Angle<fltp08>(DEGREES, 0.01638888888)
        , Angle<fltp08>(DEGREES, 0.06 / 60.0 / 60));//goniometer
    Vector<2, Angle<fltp08>> trunation = Vector<2, Angle<fltp08>>(
        Angle<fltp08>(DEGREES, 0.01638888888)
        , Angle<fltp08>(DEGREES, 0.06 / 60.0 / 60));//goniometer
    //Angle<fltp08> δπ = Angle<fltp08>(RADIANS, 0.001);
    //standing axis (SA) and trunnion axis (TA)
    class TOTAL_STATION_API TotalStationNoiseSimulator
    {
    public:
        template<uint01 t_dims, class t_type>
        void addVector(GenericOptionGroup& group, const Vector<t_dims, t_type>& vector, const StringView name, ConstPointer<Unit> unit = ConstPointer<Unit>())
        {
            for (uint04 i = 0; i < t_dims; i++)
            {
                String axis_name(name);
                axis_name = axis_name.toLower();
                axis_name.replaceAll(' ', '_');
                axis_name.append('_');
                switch (i)
                {
                case 0: axis_name.append("Hz"); break;
                case 1: axis_name.append("V"); break;
                default: axis_name.append(cast<char>('x' + i)); break;
                }
                
                GenericOption option(axis_name, TranslatedString::DirectString(axis_name.toTitleString()), cast<fltp04>(vector[i]));
                //option.dec
                option.unit = unit;
                group.addOption(option);
            }
        }
        template<uint01 t_dims, class t_type>
        void setVector(const GenericOptionGroup& group, Vector<t_dims, t_type>& vector, const StringView name)
        {
            for (uint04 i = 0; i < t_dims; i++)
            {
                String axis_name(name);
                axis_name = axis_name.toLower();
                axis_name.replaceAll(' ', '_');
                axis_name.append('_');
                switch (i)
                {
                case 0: axis_name.append("Hz"); break;
                case 1: axis_name.append("V"); break;
                default: axis_name.append(cast<char>('x' + i)); break;
                }
                vector[i] = group.getValue<fltp08>(axis_name);
            }
        }
        Buffer<GenericOptionGroup> groups()
        {
            Buffer<GenericOptionGroup> options;
 
            GenericOptionGroup camera(_t("Camera"));
            camera.addOption(GenericOption("camera_fov", _t("Camera FOV"), camera_fov));
            options.add(camera);
            //Gyroscope noise and bias parameters
            GenericOptionGroup theodolite(_t("Errors"));
            addVector(theodolite, v_circle_eccentricity, "vertical_circle_eccentricity", UnitDefinitions::Micron());
            addVector(theodolite, h_circle_eccentricity, "horizontal_circle_eccentricity", UnitDefinitions::Micron());
            GenericOption encoder_radius("encoder_radius", _t("Encoder Ring Radius"), encoder_radius);
            encoder_radius.unit = UnitDefinitions::Millimeters();
            theodolite.addOption(encoder_radius);
            addVector(theodolite, optical_axis, "optical_axis", UnitDefinitions::DegreesMinutesSeconds());
            addVector(theodolite, optical_collimation, "collimation", UnitDefinitions::DegreesMinutesSeconds());
            addVector(theodolite, trunation, "trunation", UnitDefinitions::DegreesMinutesSeconds());
            options.add(theodolite);
 
 
            return options;
            // Magnetometer noise parameters (WARNING: Datasheet is incomplete for noise density and bias, this is a best guess)
            /*fltp08 mag_noise_density_mean_ugauss_rt_hz = 80;
            fltp08 mag_noise_density_std_ugauss_rt_hz = 80;
            Vector<3, fltp08> mag_bias_gauss = Vector<3, fltp08>(0.0);
            fltp08 mag_bias_offset_mean_ugauss = 0.0016;
            fltp08 mag_bias_offset_std_ugauss = 0.0016;
            fltp08 max_mag_bias_offset_gauss = 0.0085;*/
        }
        
        void saveGyroSettings(File file)
        {
            Buffer<GenericOptionGroup> settings = groups();
            INIFactory gyro_factory;
            gyro_factory.setPreserveOrder(true);
            settings[0].addOptionsToINI(gyro_factory);
            gyro_factory.writeToAsciiFile(file);
        }
        void saveAccelerometerSettings(File file)
        {
            Buffer<GenericOptionGroup> settings = groups();
            INIFactory acc_factory;
            acc_factory.setPreserveOrder(true);
            settings[1].addOptionsToINI(acc_factory);
            acc_factory.writeToAsciiFile(file);
        }
        void loadGyroSettings(File file)
        {
            Buffer<GenericOptionGroup> settings = groups();
            INIFactory gyro_factory;
            gyro_factory.setPreserveOrder(true);
            settings[0].addOptionsToINI(gyro_factory);
            gyro_factory.readAsciiFile(file);
        }
        void loadAccelerometerSettings(File file)
        {
            Buffer<GenericOptionGroup> settings = groups();
            INIFactory acc_factory;
            acc_factory.setPreserveOrder(true);
            settings[1].addOptionsToINI(acc_factory);
            acc_factory.readAsciiFile(file);
        }
        TotalStationNoiseSimulator()
            : m_random_number_generator(Time::SystemTime().getNanoSeconds() % 100000)//random seed
        {
        }
        void setTemperature(fltp08 temperature)
        {
            //TODO: Adjust any needed variables
            m_temperature = temperature;
        }
        
    private:
        // Internal state tracking
        Vector<3, fltp08> m_current_gyro_scale_error = Vector<3, fltp08>(0.0);
        Vector<3, fltp08> m_current_accel_bias_instability = Vector<3, fltp08>(0.0);
        Vector<3, fltp08> m_gyro_bias_dph = Vector<3, fltp08>(0.0); // Current gyro bias per axis in deg/hr
        Vector<3, fltp08> m_accel_bias = Vector<3, fltp08>(0.0); // Current accel bias per axis in ug
        Vector<3, fltp08> m_accel_scale_error = Vector<3, fltp08>(0.0);
        GaussianRN<> m_random_number_generator;//Used to generate random gausian numbers by calling randonumber_generator.get() 
        Time m_last_gyro_update_time = Constant<Time>::Invalid;
        Time m_last_accelerometer_update_time = Constant<Time>::Invalid;
        Time m_last_magnetometer_update_time = Constant<Time>::Invalid;
        fltp08 m_temperature = 30.0;//temperature in celsius. Can be adjusted in the simulation.
    };
}