 |
WarpTwin
Documentation for WarpTwin models and classes.
|
|
WarpTwin
Documentation for WarpTwin models and classes.
|
/******************************************************************************
* Copyright (c) ATTX INC 2025. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the
* proprietary and confidential information of ATTX INC. The Software is
* furnished under a license agreement between ATTX and the user organization
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or
* transmitted in any form or by any means, for any purpose, without the express
* written permission of ATTX INC.
******************************************************************************/
/*
Gyro model header file
Author: Alex Reynolds
*/
/*
Metadata for MS GUI:
imdata = {"displayname" : "Gyro",
"exclude" : False,
"category" : "Sensors"
}
aliases = {"bias_initial" : "Bias",
"min_ang_vel" : "Minimum Ang. Vel.",
"max_ang_vel" : "Maximum Ang. Vel.",
"mount_frame" : "Spacecraft Body",
"mount_position__mf" : "Body Mount Position",
"mount_alignment_mf" : "Body Mount Alignment",
"resolution" : "EXCLUDE",
"rate_hz" : "Rate Hz",
"seed_value" : "EXCLUDE",
"latency" : "EXCLUDE",
"gaussian_noise" : "Noise",
"walking_bias_std" : "EXCLUDE",
"scale_factor_std" : "EXCLUDE",
"meas_ang_vel_sf" : "Meas. Ang. Vel.",
"perfect_ang_vel_sf" : "EXCLUDE",
"is_valid" : "Meas. Validity Flag",
"operational_power_draw" : "Operational Power Draw",
"mass" : "Mass",
"current_power_draw" : "Current Power Draw",
}
*/
#ifndef MODELS_SENSORS_GYROSCOPE_H
#define MODELS_SENSORS_GYROSCOPE_H
#include "simulation/Model.h"
#include "constants/unitutils.h"
#include "models/support/MarkovUncertaintyModel.h"
#include "monitors/RateMonitor.h"
#include "utils/LatencyUtil.hpp"
namespace warptwin {
/**
* @brief Gyro Model
*
* This model simulates a simple gyroscope with bias and noise
*
* HOW DOES THIS MODEL HANDLE NOISE:
* The typical 3-axis gyroscope has uncorrelated noise in each of the three
* measurement axes. The additive noise in the gyroscope can be modeled as
* Gaussian white noise with a standard deviation that is dependent on measurement
* rate, temperature, and oversampling ratio.
*
* The typical gyroscope also has some bias even after factory calibration. This
* bias is not completely stable so the bias random walk does need to be modeled for
* the majority of missions with a duration. The bias random walk standard deviation
* is constant as a function of time, however it is usually not constant as a function
* of temperature. Most gyroscope data sheets will have temperature effects on the
* bias stability.
*
* The majority of gyroscopes exhibit some amount of multiplicate noise that is
* very temperature and age dependent. This is due to the Micro-Electrical-Mechanical
* systems that are in the sensor being expanded and decompressed with temperature
* effects. Additionally, the internal polynomial function that is used to convert the
* measured voltage to an acceleration through an ADC is subject to manufacturing
* tolerances. The percent error standard deviation should be very small, around 0.1%-0.5%
*
* Typically the noise profile (additive, multiplicative, and biasing) are all dependent
* on temperature. Because of this, the model has the values as inputs. The user has the
* choice to repeatedly input new noise characteristics based on an external model, or input
* them once and it be held constant.
*
* Author: Alex Reynolds <alex.reynolds@attx.tech>
* Updated: James Tabony <james.tabony@attx.tech>
* Updated sensor noise, added dead-zones, added quantinization, added latency
*/
/// @brief Sensor output struct for latency model, its members are the same as the model outputs. Its default constructor populates members with defualt output values
struct _gyro_output_struct {
CartesianVector3 meas_ang_vel_sf;
CartesianVector3 bias;
bool is_valid;
// Default constuctor (should populate with model defualt outputs)
_gyro_output_struct()
: meas_ang_vel_sf(CartesianVector3({0.0, 0.0, 0.0})),
bias(CartesianVector3({0.0, 0.0, 0.0})),
is_valid(false) {}
// Custom constructor
_gyro_output_struct(CartesianVector3 meas_ang_vel_sf, CartesianVector3 bias, bool is_valid)
: meas_ang_vel_sf(meas_ang_vel_sf),
bias(bias),
is_valid(is_valid) {}
};
MODEL(Gyroscope)
public:
// Model params
// NAME TYPE DEFAULT VALUE
START_PARAMS
/** The initial bias in gyroscope measurement output described as a three-element vector in radians/second.
* Default is no bias. Must be set before model startup. */
SIGNAL(bias_initial, CartesianVector3, CartesianVector3({0.0, 0.0, 0.0}))
/** The minimum angular velocity that the sensor can record in each axis. Default is -540 degrees/second (radians/second) */
SIGNAL(min_ang_vel, double, -540.0*warpos::DEGREES_TO_RADIANS)
/** The maximum angular velocity that the sensor can record in each axis. Default is 540 degrees/second (radians/second) */
SIGNAL(max_ang_vel, double, 540.0*warpos::DEGREES_TO_RADIANS)
/** The vehicle frame relative to which the sensor is mounted and aligned. This is most
* typically the body frame of a spacecraft or other vehicle. mount_position__mf and mount_alignment_mf
* are described relative to this frame. */
SIGNAL(mount_frame, Frame*, nullptr)
/** The position of the sensor in the mount frame, represented in the default simulation
* unit (meters by default. pretty much always meters) */
SIGNAL(mount_position__mf, CartesianVector3, CartesianVector3({0.0, 0.0, 0.0}))
/** The alignment of the gyroscope relative to the mount frame */
SIGNAL(mount_alignment_mf, clockwerk::Quaternion, clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
/** The measurement resolution. The output value will be some multiple of this value. If the
* value is left at zero, an infinite resolution will be assumed, i.e. no data loss/quantinization.
* From a data sheet, this value can be computed as (measurement range) / 2^(bits per measurement) . (radians/second) */
SIGNAL(resolution, double, 0.0)
/** The rate at which the sensor generates an output, in hertz. This value must be some
* positive (non-zero) integer. (Hz) */
SIGNAL(rate_hz, int, 0)
/** Value to seed the internal RNG for this model. */
SIGNAL(seed_value, int, 0)
/** Latency of the pressure sensor, millisecond value for the amount of delay in sim time for
* the correct values to be reflected in the outputs. Must be set before executive startup.
* Defaults to no delay. (milliseconds) */
SIGNAL(latency, int, 0)
/** Power draw of the gyroscope. This value may or may not be the peak power draw provided by most
* data sheets. This value is the expected power draw of a sensor when operational. (Watts) */
SIGNAL(operational_power_draw, double, 0.0)
/** Mass of the gyroscope. (kg) */
SIGNAL(mass, double, 0.0)
END_PARAMS
// Model inputs
// NAME TYPE DEFAULT VALUE
START_INPUTS
/** The one-sigma Gaussian noise in gyroscope measurement output described as a three-element vector
* in radians/second. Default is no noise. */
SIGNAL(gaussian_noise, CartesianVector3, CartesianVector3({0.0, 0.0, 0.0}))
/** The one-sigma Gaussian noise in the gyroscope bias drift described as a three-element vector
* in radians/second/sqrt(second). Default is no walking-bias drift. */
SIGNAL(walking_bias_std, CartesianVector3, CartesianVector3({0.0, 0.0, 0.0}))
/** The one-sigma scale percent increase/decrease of the measurement. Default is no scaling. */
SIGNAL(scale_factor_std, CartesianVector3, CartesianVector3({0.0, 0.0, 0.0}))
END_INPUTS
// Model outputs
// NAME TYPE DEFAULT VALUE
START_OUTPUTS
/** Output time tied to measurements. Output time is exactly the navigation time
* (see SimTimeManager for configuration), without latency (instantaneous) but
* but with a rate that mirrors output rate */
SIGNAL(output_time, clockwerk::Time, clockwerk::Time(0, 0))
/** The measured output angular velocity produced by the gyro describing the
* angular velocity of the sensor frame relative to the simulation root frame
* appropriate bias, noise, latency, and rate limiting. (radians/second) */
SIGNAL(meas_ang_vel_sf, CartesianVector3, CartesianVector3({0.0, 0.0, 0.0}))
/** The "perfect" output angular velocity produced by the gyro describing the
* angular velocity of the sensor frame (incl. misalignment) relative to the
* simulation root frame without bias and noise. This is an informational
* parameter and thus is not subject to latency. (radians/second) */
SIGNAL(perfect_ang_vel_sf, CartesianVector3, CartesianVector3({0.0, 0.0, 0.0}))
/** Boolean value to notify if the output measurement is valid (i.e. boolean for
* blackout-zones). True = not in blackout-zone, False = in blackout-zone. */
SIGNAL(is_valid, bool, false)
/** Power draw of the sensor. This value is populated when the model is active, and zero
* when the model is deactive. Allows the user to create duty cycles and power budgets. (Watts) */
SIGNAL(current_power_draw, double, 0.0)
/** Current bias of the gyroscope. (rad/s) */
SIGNAL(bias, CartesianVector3, CartesianVector3({0.0, 0.0, 0.0}))
END_OUTPUTS
/// @brief Accessor for the sensor's frame
clockwerk::DataIO<Frame*> sensor_frame = clockwerk::DataIO<Frame*>(this, "sensor_frame", &_sensor_frame);
/// @brief Accessor for the internal noise model
/// @return Pointer to the internal noise model
warptwin::MarkovUncertaintyModel* getNoiseModel() {return &_sensor_noise_model;}
/// @brief Accessor for the internal rate monitor model
/// @return Pointer to the rate monitor model
warptwin::RateMonitor* rateMonitor() {return &_rate_monitor;}
int16 activate() override;
int16 deactivate() override;
protected:
int16 start() override;
int16 execute() override;
/// @brief Function to configure sensor -- runs in all constructors
void _configureInternal();
/// @brief The sensor frame in which all measurements will be taken
Frame _sensor_frame;
/// @brief The bias and noise model for sensor output.
MarkovUncertaintyModel _sensor_noise_model;
/// @brief Rate monitor to control the rate at which the sensor runs
RateMonitor _rate_monitor;
/// @brief Temporary variable to hold total error for gyro
CartesianVector3 _total_error;
/// @brief Temporary variable to hold the bias vector at last function call
CartesianVector3 _previous_bias;
/// @brief Temporary variable to hold the perturbed ang vel
CartesianVector3 _perturbed_ang_vel;
/// @brief Temporary variable for checking if sensor is in deadzone. Each element of vector corresponds to an axis
std::array<bool, 3> _in_deadzone;
/// @brief Internal latency model, templated to the sensor output struct
LatencyUtil<_gyro_output_struct> _latency_model;
/// @brief Temporary variable for the latest recorded output added to latency model and dummy output for stepping though latency
_gyro_output_struct _last_output, _latency_return;
};
}
#endif
1.16.1