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

HAL_SITL: removed earth-frame rates

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-25 08:42:37 +10:00
parent 0d20167294
commit 8dce5e11d6
2 changed files with 6 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_HAL_SITL/sitl_ins.cpp
View File

@ -151,17 +151,11 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative
double xAccel, double yAccel, double zAccel, // Local to plane
float airspeed, float altitude)
{
double p, q, r;
if (_ins == NULL) {
// no inertial sensor in this sketch
return;
}
SITL::convert_body_frame(roll, pitch,
rollRate, pitchRate, yawRate,
&p, &q, &r);
// minimum noise levels are 2 bits, but averaged over many
// samples, giving around 0.01 m/s/s
float accel_noise = 0.01f;
@ -191,9 +185,9 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative
_ins->set_accel(0, Vector3f(xAccel1, yAccel1, zAccel1) + _ins->get_accel_offsets(0));
_ins->set_accel(1, Vector3f(xAccel2, yAccel2, zAccel2) + _ins->get_accel_offsets(1));
p += _gyro_drift();
q += _gyro_drift();
r += _gyro_drift();
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();

12
libraries/AP_HAL_SITL/sitl_optical_flow.cpp
View File

@ -33,7 +33,6 @@ static OpticalFlow::OpticalFlow_state optflow_data[MAX_OPTFLOW_DELAY];
*/
void SITL_State::_update_flow(void)
{
double p, q, r;
Vector3f gyro;
static uint32_t last_flow_ms;
@ -49,14 +48,9 @@ void SITL_State::_update_flow(void)
}
last_flow_ms = now;
// convert roll rates to body frame
SITL::convert_body_frame(_sitl->state.rollDeg,
_sitl->state.pitchDeg,
_sitl->state.rollRate,
_sitl->state.pitchRate,
_sitl->state.yawRate,
&p, &q, &r);
gyro(p, q, r);
gyro(radians(_sitl->state.rollRate),
radians(_sitl->state.pitchRate),
radians(_sitl->state.yawRate));
OpticalFlow::OpticalFlow_state state;