HAL_SITL: moved virtual INS sensor to AP_InertialSensor_SITL

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -315,6 +315,7 @@ void SITL_State::_fdm_input_local(void)
 
     // get FDM output from the model
     sitl_model->fill_fdm(_sitl->state);
+    _sitl->update_rate_hz = sitl_model->get_rate_hz();
 
     if (gimbal != NULL) {
         gimbal->update();
@@ -418,7 +419,7 @@ void SITL_State::_simulator_servos(Aircraft::sitl_input &input)
             input.servos[2] = ((input.servos[2]-1000) * _sitl->engine_mul) + 1000;
             if (input.servos[2] > 2000) input.servos[2] = 2000;
         }
-        _motors_on = ((input.servos[2]-1000)/1000.0f) > 0;
+        _sitl->motors_on = ((input.servos[2]-1000)/1000.0f) > 0;
     } else if (_vehicle == APMrover2) {
         // add in engine multiplier
         if (input.servos[2] != 1500) {
@@ -426,9 +427,9 @@ void SITL_State::_simulator_servos(Aircraft::sitl_input &input)
             if (input.servos[2] > 2000) input.servos[2] = 2000;
             if (input.servos[2] < 1000) input.servos[2] = 1000;
         }
-        _motors_on = ((input.servos[2]-1500)/500.0f) != 0;
+        _sitl->motors_on = ((input.servos[2]-1500)/500.0f) != 0;
     } else {
-        _motors_on = false;
+        _sitl->motors_on = false;
         // apply engine multiplier to first motor
         input.servos[0] = ((input.servos[0]-1000) * _sitl->engine_mul) + 1000;
         // run checks on each motor
@@ -438,12 +439,12 @@ void SITL_State::_simulator_servos(Aircraft::sitl_input &input)
             if (input.servos[i] < 1000) input.servos[i] = 1000;
             // update motor_on flag
             if ((input.servos[i]-1000)/1000.0f > 0) {
-                _motors_on = true;
+                _sitl->motors_on = true;
             }
         }
     }
 
-    float throttle = _motors_on?(input.servos[2]-1000) / 1000.0f:0;
+    float throttle = _sitl->motors_on?(input.servos[2]-1000) / 1000.0f:0;
     // lose 0.7V at full throttle
     float voltage = _sitl->batt_voltage - 0.7f*throttle;
 

libraries/AP_HAL_SITL/SITL_State.h

@@ -123,7 +123,6 @@ private:
     void _apply_servo_filter(float deltat);
     uint16_t _airspeed_sensor(float airspeed);
     uint16_t _ground_sonar();
-    float _gyro_drift(void);
     float _rand_float(void);
     Vector3f _rand_vec3f(void);
     void _fdm_input_step(void);
@@ -138,7 +137,6 @@ private:
     struct sockaddr_in _rcout_addr;
     pid_t _parent_pid;
     uint32_t _update_count;
-    bool _motors_on;
 
     AP_Baro *_barometer;
     AP_InertialSensor *_ins;

libraries/AP_HAL_SITL/sitl_ins.cpp

@@ -77,21 +77,6 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed)
 }
 
 
-float SITL_State::_gyro_drift(void)
-{
-    if (_sitl->drift_speed == 0.0f ||
-            _sitl->drift_time == 0.0f) {
-        return 0;
-    }
-    double period  = _sitl->drift_time * 2;
-    double minutes = fmod(_scheduler->_micros64() / 60.0e6, period);
-    if (minutes < period/2) {
-        return minutes * ToRad(_sitl->drift_speed);
-    }
-    return (period - minutes) * ToRad(_sitl->drift_speed);
-
-}
-
 /*
   emulate an analog rangefinder
  */
@@ -156,59 +141,6 @@ void SITL_State::_update_ins(float roll, 	float pitch, 	float yaw,		// Relative
         return;
     }
 
-    // minimum noise levels are 2 bits, but averaged over many
-    // samples, giving around 0.01 m/s/s
-    float accel_noise = 0.01f;
-    float accel2_noise = 0.01f;
-    // minimum gyro noise is also less than 1 bit
-    float gyro_noise = ToRad(0.04f);
-    if (_motors_on) {
-        // add extra noise when the motors are on
-        accel_noise += _sitl->accel_noise;
-        accel2_noise += _sitl->accel2_noise;
-        gyro_noise += ToRad(_sitl->gyro_noise);
-    }
-    // get accel bias (add only to first accelerometer)
-    Vector3f accel_bias = _sitl->accel_bias.get();
-    float xAccel1 = xAccel + accel_noise * _rand_float() + accel_bias.x;
-    float yAccel1 = yAccel + accel_noise * _rand_float() + accel_bias.y;
-    float zAccel1 = zAccel + accel_noise * _rand_float() + accel_bias.z;
-
-    float xAccel2 = xAccel + accel2_noise * _rand_float();
-    float yAccel2 = yAccel + accel2_noise * _rand_float();
-    float zAccel2 = zAccel + accel2_noise * _rand_float();
-
-    if (fabsf(_sitl->accel_fail) > 1.0e-6f) {
-        xAccel1 = _sitl->accel_fail;
-        yAccel1 = _sitl->accel_fail;
-        zAccel1 = _sitl->accel_fail;
-    }
-
-    Vector3f accel0 = Vector3f(xAccel1, yAccel1, zAccel1) + _ins->get_accel_offsets(0);
-    Vector3f accel1 = Vector3f(xAccel2, yAccel2, zAccel2) + _ins->get_accel_offsets(1);
-    _ins->set_accel(0, accel0);
-    _ins->set_accel(1, accel1);
-
-    // check noise
-    _ins->calc_vibration_and_clipping(0, accel0, 0.0025f);
-    _ins->calc_vibration_and_clipping(1, accel1, 0.0025f);
-
-    float p = radians(rollRate) + _gyro_drift();
-    float q = radians(pitchRate) + _gyro_drift();
-    float r = radians(yawRate) + _gyro_drift();
-
-    float p1 = p + gyro_noise * _rand_float();
-    float q1 = q + gyro_noise * _rand_float();
-    float r1 = r + gyro_noise * _rand_float();
-
-    float p2 = p + gyro_noise * _rand_float();
-    float q2 = q + gyro_noise * _rand_float();
-    float r2 = r + gyro_noise * _rand_float();
-
-    _ins->set_gyro(0, Vector3f(p1, q1, r1) + _ins->get_gyro_offsets(0));
-    _ins->set_gyro(1, Vector3f(p2, q2, r2) + _ins->get_gyro_offsets(1));
-
-
     sonar_pin_value    = _ground_sonar();
     float airspeed_simulated = (fabsf(_sitl->aspd_fail) > 1.0e-6f) ? _sitl->aspd_fail : airspeed;
     airspeed_pin_value = _airspeed_sensor(airspeed_simulated + (_sitl->aspd_noise * _rand_float()));