void AP_AHRS_NavEKF::update_SITL(void)
{
if (_sitl == nullptr) {
_sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
if (_sitl == nullptr) {
return;
}
}
const struct SITL::sitl_fdm &fdm = _sitl->state;
if (active_EKF_type() == EKF_TYPE_SITL) {
roll = radians(fdm.rollDeg);
pitch = radians(fdm.pitchDeg);
yaw = radians(fdm.yawDeg);
fdm.quaternion.rotation_matrix(_dcm_matrix);
update_cd_values();
update_trig();
_gyro_drift.zero();
_gyro_estimate = Vector3f(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
const Vector3f accel(fdm.xAccel,
fdm.yAccel,
fdm.zAccel);
_accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
_accel_ef_ekf_blended = _accel_ef_ekf[0];
}
if (_sitl->odom_enable) {
// use SITL states to write body frame odometry data at 20Hz
uint32_t timeStamp_ms = AP_HAL::millis();
if (timeStamp_ms - _last_body_odm_update_ms > 50) {
const float quality = 100.0f;
const Vector3f posOffset(0.0f, 0.0f, 0.0f);
const float delTime = 0.001f * (timeStamp_ms - _last_body_odm_update_ms);
_last_body_odm_update_ms = timeStamp_ms;
timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay
Vector3f delAng(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
delAng *= delTime;
// rotate earth velocity into body frame and calculate delta position
Matrix3f Tbn;
Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));
const Vector3f earth_vel(fdm.speedN,fdm.speedE,fdm.speedD);
const Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
// write to EKF
EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
}
}
}
void
Compass::null_offsets(const Matrix3f &dcm_matrix)
{
// Update our estimate of the offsets in the magnetometer
Vector3f calc(0.0, 0.0, 0.0); // XXX should be safe to remove explicit init here as the default ctor should do the right thing
Matrix3f dcm_new_from_last;
float weight;
Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z);
if(_null_init_done) {
dcm_new_from_last = dcm_matrix.transposed() * _last_dcm_matrix; // Note 11/20/2010: transpose() is not working, transposed() is.
weight = 3.0 - fabs(dcm_new_from_last.a.x) - fabs(dcm_new_from_last.b.y) - fabs(dcm_new_from_last.c.z);
if (weight > .001) {
calc = mag_body_new + _mag_body_last; // Eq 11 from Bill P's paper
calc -= dcm_new_from_last * _mag_body_last;
calc -= dcm_new_from_last.transposed() * mag_body_new;
if(weight > 0.5) weight = 0.5;
calc = calc * (weight);
_offset.set(_offset.get() - calc);
}
} else {
_null_init_done = true;
}
_mag_body_last = mag_body_new - calc;
_last_dcm_matrix = dcm_matrix;
}
Surface makeSurfRev(const Curve &profile, unsigned steps)
{
Surface surface;
vector<Vector3f> initialVV;
if (!checkFlat(profile))
{
cerr << "surfRev profile curve must be flat on xy plane." << endl;
exit(0);
}
// TODO: Here you should build the surface. See surf.h for details.
//cerr << "\t>>> makeSurfRev called (but not implemented).\n\t>>> Returning empty surface." << endl;
for (unsigned i=0; i<profile.size(); i++){
for (unsigned j=0; j<=steps; j++){
float t = 2.0f * M_PI * float( j ) / steps;
//Matrix operations used to calulate normal
Matrix4f rotatM = Matrix4f::rotateY(t);
Matrix3f rotatMsub = rotatM.getSubmatrix3x3(0,0);
Matrix3f rotatMtrans = rotatMsub.transposed();
Matrix3f rotatN = rotatMtrans.inverse();
//Calculate surface vertex
Vector4f surfaceCalc = Vector4f(profile[i].V[0], profile[i].V[1], profile[i].V[2], 1.f);
Vector4f surfaceVecInit = rotatM*surfaceCalc;
Vector3f surfaceVec = Vector3f(surfaceVecInit[0], surfaceVecInit[1], surfaceVecInit[2]);
//Calculate surface normal
Vector3f surfaceVNInit = rotatN*profile[i].N;
//Push vectors into surface data
surface.VV.push_back(surfaceVec);
surface.VN.push_back(-1*surfaceVNInit);
}
}
//Calculate faces once all the vertices are added
for (unsigned k=0; k<surface.VV.size()-(steps+1);k++){
Tup3u firstTri; //faces uses a series of connected triangles
Tup3u secondTri;
if ((k+1)%(steps+1) != 0) //Create triangles (considering edge conditions)
{
//Triangles in counter-clockwise manner
firstTri = Tup3u(k+1, k, k+steps+1);
secondTri = Tup3u(k+1, k+1+steps, k+2+steps);
}
surface.VF.push_back(firstTri);
surface.VF.push_back(secondTri);
}
return surface;
}
/*
given a magnetic heading, and roll, pitch, yaw values,
calculate consistent magnetometer components
All angles are in radians
*/
static Vector3f heading_to_mag(float roll, float pitch, float yaw)
{
Vector3f Bearth, m;
Matrix3f R;
// Bearth is the magnetic field in Canberra. We need to adjust
// it for inclination and declination
Bearth(MAG_FIELD_STRENGTH, 0, 0);
R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(MAG_DECLINATION));
Bearth = R * Bearth;
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
return m + (rand_vec3f() * sitl.mag_noise);
}
void AC_AttitudeControl::get_rotation_reference_to_vehicle(Matrix3f& m)
{
get_rotation_vehicle_to_reference(m);
m = m.transposed();
}
void QD3D11MultiViewportViewer::mouseMoveEvent( QMouseEvent* event )
{
Vector2i curPos( event->x(), event->y() );
Vector2f delta = curPos - m_prevPos;
#if 1
float pitchSpeed = m_flipMouseUpDown ? m_mousePitchSpeed : -m_mousePitchSpeed;
const float yawSpeed = 0.005f;
const float panSpeed = 0.005f;
const float walkSpeed = -0.005f;
Matrix3f worldToCamera = m_perspectiveCamera.viewMatrix().getSubmatrix3x3( 0, 0 );
Matrix3f cameraToWorld = m_perspectiveCamera.inverseViewMatrix().getSubmatrix3x3( 0, 0 );
Vector3f eye = m_perspectiveCamera.eye();
Vector3f x = m_perspectiveCamera.right();
Vector3f y = m_perspectiveCamera.up();
Vector3f z = m_perspectiveCamera.forward();
// rotate
if( event->buttons() == Qt::LeftButton )
{
// pitch around the local x axis
float pitch = pitchSpeed * delta.y;
Matrix3f pitchMatrix = Matrix3f::rotateX( pitch );
y = cameraToWorld * pitchMatrix * worldToCamera * y;
z = cameraToWorld * pitchMatrix * worldToCamera * z;
// yaw around the world up vector
float yaw = yawSpeed * delta.x;
Matrix3f yawMatrix = m_groundPlaneToWorld * Matrix3f::rotateY( yaw ) * m_worldToGroundPlane;
x = yawMatrix * x;
y = yawMatrix * y;
z = yawMatrix * z;
m_perspectiveCamera.setLookAt( eye, eye + z, y );
}
// walk
else if( event->buttons() == Qt::RightButton )
{
float dx = panSpeed * delta.x;
float dz = walkSpeed * delta.y;
translate( dx, 0, dz );
}
// move up/down
else if( event->buttons() == Qt::MiddleButton )
{
float dy = -panSpeed * delta.y;
translate( 0, dy, 0 );
}
#else
if(event->buttons() & Qt::RightButton) //rotate
{
float rotSpeed = 0.005f; //radians per pixel
Quat4f rotation;
rotation.setAxisAngle(rotSpeed * delta.abs(), Vector3f(-delta[1], -delta[0], 0));
Matrix3f rotMatrix = Matrix3f::rotation(rotation);
Matrix3f viewMatrix = m_camera.getViewMatrix().getSubmatrix3x3(0, 0);
rotMatrix = viewMatrix.transposed() * rotMatrix * viewMatrix;
Vector3f eye, center, up;
m_camera.getLookAt(&eye, ¢er, &up);
m_camera.setLookAt(center + rotMatrix * (eye - center), center, rotMatrix * up);
}
else if(event->buttons() & Qt::LeftButton) //translate
{
float speed = 10.f;
Vector3f screenDelta(delta[0], delta[1], 0);
screenDelta[0] /= -double(width());
screenDelta[1] /= double(height());
Matrix4f iViewProjMatrix = m_camera.getInverseViewProjectionMatrix();
Vector3f worldDelta = iViewProjMatrix.getSubmatrix3x3(0, 0) * (speed * screenDelta);
Vector3f eye, center, up;
m_camera.getLookAt(&eye, ¢er, &up);
m_camera.setLookAt(eye + worldDelta, center + worldDelta, up);
}
#endif
m_prevPos = curPos;
update();
}
Surface makeGenCyl(const Curve &profile, const Curve &sweep )
{
Surface surface;
if (!checkFlat(profile))
{
cerr << "genCyl profile curve must be flat on xy plane." << endl;
exit(0);
}
// TODO: Here you should build the surface. See surf.h for details.
//cerr << "\t>>> makeGenCyl called (but not implemented).\n\t>>> Returning empty surface." <<endl;
for (unsigned i=0; i<profile.size(); i++){
for (unsigned j=0; j<sweep.size(); j++){
//Matrix from sweep
Matrix4f coordM(sweep[j].N[0], sweep[j].B[0], sweep[j].T[0], sweep[j].V[0],
sweep[j].N[1], sweep[j].B[1], sweep[j].T[1], sweep[j].V[1],
sweep[j].N[2], sweep[j].B[2], sweep[j].T[2], sweep[j].V[2],
0.f, 0.f, 0.f, 1.f);
//Matrix operations to get normal
Matrix3f rotatMsub = coordM.getSubmatrix3x3(0,0);
Matrix3f rotatMtrans = rotatMsub.transposed();
Matrix3f rotatN = rotatMtrans.inverse();
//Calculate surface vertex
Vector4f surfaceCalc = Vector4f(profile[i].V[0], profile[i].V[1], profile[i].V[2], 1.f);
Vector4f surfaceVecInit = coordM*surfaceCalc;
Vector3f surfaceVec = Vector3f(surfaceVecInit[0], surfaceVecInit[1], surfaceVecInit[2]);
//Calculate surface normal
Vector3f surfaceVNInit = rotatN*profile[i].N;
//Push vectors into surface data
surface.VV.push_back(surfaceVec);
surface.VN.push_back(-1*surfaceVNInit);
}
}
//Calculate faces once all the vertices are added
for (unsigned k=0; k<surface.VV.size()-(sweep.size());k++){
Tup3u firstTri; //faces uses a series of connected triangles
Tup3u secondTri;
if ((k+1)%(sweep.size()) != 0) //Create triangles (considering edge conditions)
{
//Triangles in counter-clockwise manner
firstTri = Tup3u(k+1, k, k+sweep.size());
secondTri = Tup3u(k+1, k+sweep.size(), k+1+sweep.size());
}
surface.VF.push_back(firstTri);
surface.VF.push_back(secondTri);
}
return surface;
}
