From 740456aec42d56088e8c5f13ca729fcab087c933 Mon Sep 17 00:00:00 2001
From: Eddy Scott <scott.edward@aurora.aero>
Date: Wed, 12 Aug 2015 17:07:12 -0400
Subject: [PATCH] Added setter functions for HIL noise variances.  Researching
 methods to add zero mean gaussian random number before implementation

---
 src/uas/UAS.cc | 23 ++++++++++++++++
 src/uas/UAS.h  | 71 ++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 94 insertions(+)

diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index e51de03ff..bf3c3f8e0 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -143,6 +143,20 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     blockHomePositionChanges(false),
     receivedMode(false),
 
+    xacc_var(0.0),
+    yacc_var(0.0),
+    zacc_var(0.0),
+    rollspeed_var(0.0),
+    pitchspeed_var(0.0),
+    yawspeed_var(0.0),
+    xmag_var(0.0),
+    ymag_var(0.0),
+    zmag_var(0.0),
+    abs_pressure_var(0.0),
+    diff_pressure_var(0.0),
+    pressure_alt_var(0.0),
+    temperature_var(0.0),
+
 #ifndef __mobile__
     simulation(0),
 #endif
@@ -2989,6 +3003,8 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
 {
     if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
     {
+
+
         mavlink_message_t msg;
         mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
                                    time_us, xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
@@ -3006,6 +3022,13 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
 }
 #endif
 
+#ifndef __mobile__
+float UAS::addZeroMeanNoise(float truth_meas, noise_var)
+{
+    
+}
+#endif
+
 #ifndef __mobile__
 void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
                     float flow_comp_m_y, quint8 quality, float ground_distance)
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 85a515f6d..a7ff66e08 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -332,6 +332,60 @@ public:
         return nedAttGlobalOffset;
     }
 
+
+    // Setters for HIL noise variance
+    void setXaccVar(float var){
+        xacc_var = var;
+    }
+
+    void setYaccVar(float var){
+        yacc_var = var;
+    }
+
+    void setZaccVar(float var){
+        zacc_var = var;
+    }
+
+    void setRollSpeedVar(float var){
+        rollspeed_var = var;
+    }
+
+    void setPitchSpeedVar(float var){
+        pitchspeed_var = var;
+    }
+
+    void setYawSpeedVar(float var){
+        pitchspeed_var = var;
+    }
+
+    void setXmagVar(float var){
+        xmag_var = var;
+    }
+
+    void setYmagVar(float var){
+        ymag_var = var;
+    }
+
+    void setZmagVar(float var){
+        zmag_var = var;
+    }
+
+    void setAbsPressureVar(float var){
+        abs_pressure_var = var;
+    }
+
+    void setDiffPressureVar(float var){
+        diff_pressure_var = var;
+    }
+
+    void setPressureAltVar(float var){
+        pressure_alt_var = var;
+    }
+
+    void setTemperatureVar(float var){
+        temperature_var = var;
+    }
+
     bool isRotaryWing();
     bool isFixedWing();
 
@@ -465,6 +519,21 @@ protected: //COMMENTS FOR TEST UNIT
     bool blockHomePositionChanges;   ///< Block changes to the home position
     bool receivedMode;          ///< True if mode was retrieved from current conenction to UAS
 
+    /// SIMULATION NOISE
+    float xacc_var;             ///< variance of x acclerometer noise for HIL sim (mg)
+    float yacc_var;             ///< variance of y acclerometer noise for HIL sim (mg)
+    float zacc_var;             ///< variance of z acclerometer noise for HIL sim (mg)
+    float rollspeed_var;        ///< variance of x gyroscope noise for HIL sim (rad/s)
+    float pitchspeed_var;       ///< variance of y gyroscope noise for HIL sim (rad/s)
+    float yawspeed_var;         ///< variance of z gyroscope noise for HIL sim (rad/s)
+    float xmag_var;             ///< variance of x magnatometer noise for HIL sim (???)
+    float ymag_var;             ///< variance of y magnatometer noise for HIL sim (???)
+    float zmag_var;             ///< variance of z magnatometer noise for HIL sim (???)
+    float abs_pressure_var;     ///< variance of absolute pressure noise for HIL sim (hPa)
+    float diff_pressure_var;    ///< variance of differential pressure noise for HIL sim (hPa)
+    float pressure_alt_var;     ///< variance of altitude pressure noise for HIL sim (hPa)
+    float temperature_var;      ///< variance of temperature noise for HIL sim (C)
+
     /// SIMULATION
 #ifndef __mobile__
     QGCHilLink* simulation;         ///< Hardware in the loop simulation link
@@ -728,6 +797,8 @@ public slots:
     void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
                             float flow_comp_m_y, quint8 quality, float ground_distance);
 
+    float addZeroMeanNoise(float truth_meas, float noise_var);
+
     /**
      * @param time_us
      * @param lat
-- 
2.22.0