aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_SITL: moved virtual INS sensor to AP_InertialSensor_SITL

Browse Source
This commit is contained in:
Andrew Tridgell 2015-11-16 15:09:58 +11:00
parent 4a768d47f3
commit 95ca0b39a8
3 changed files with 6 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -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;

2
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -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;

68
libraries/AP_HAL_SITL/sitl_ins.cpp
View File

@ -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()));