From 95ca0b39a8167227745863894916da5d59864e74 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 Nov 2015 15:09:58 +1100 Subject: [PATCH] HAL_SITL: moved virtual INS sensor to AP_InertialSensor_SITL --- libraries/AP_HAL_SITL/SITL_State.cpp | 11 +++-- libraries/AP_HAL_SITL/SITL_State.h | 2 - libraries/AP_HAL_SITL/sitl_ins.cpp | 68 ---------------------------- 3 files changed, 6 insertions(+), 75 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index c3c4e9d831..bcbddb55e4 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/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; diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index d54b0772c6..7dca56615d 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/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; diff --git a/libraries/AP_HAL_SITL/sitl_ins.cpp b/libraries/AP_HAL_SITL/sitl_ins.cpp index 49b1b779c7..65f5fdd62a 100644 --- a/libraries/AP_HAL_SITL/sitl_ins.cpp +++ b/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()));