Camera camera_new(Vector3D pos,Vector3D aim,Vector3D up,float fov,float ratio){
Camera camera;
camera.m_pos = pos;
Vector3D z = vector3d_normalize(vector3d_sub(aim,pos));
camera.x = vector3d_normalize(vector3d_cross_product(z,up));
camera.y = vector3d_mul_float(vector3d_cross_product(camera.x,z),1.f/ratio);
camera.m_center = vector3d_add(camera.m_pos,vector3d_mul_float(z,(1.f / tan((fov * 3.14159265358979323846 / 180.f) * 0.5f))));
return camera;
}
/* bring dcm matrix in order - adjust values to make
* orthonormal (or at least closer to orthonormal) */
static void dcm_orthonormalize(float dcm[3][3]){
//err = X . Y , X = X - err/2 * Y , Y = Y - err/2 * X (DCMDraft2 Eqn.19)
float err = vector3d_dot((float*)(dcm[0]),(float*)(dcm[1]));
float delta[2][3];
vector3d_scale(-err/2,(float*)(dcm[1]),(float*)(delta[0]));
vector3d_scale(-err/2,(float*)(dcm[0]),(float*)(delta[1]));
vector3d_add((float*)(dcm[0]),(float*)(delta[0]),(float*)(dcm[0]));
vector3d_add((float*)(dcm[1]),(float*)(delta[1]),(float*)(dcm[1]));
//Z = X x Y (DCMDraft2 Eqn. 20) ,
vector3d_cross((float*)(dcm[0]),(float*)(dcm[1]),(float*)(dcm[2]));
//re-nomralization
vector3d_normalize((float*)(dcm[0]));
vector3d_normalize((float*)(dcm[1]));
vector3d_normalize((float*)(dcm[2]));
}
Ray camera_ray(Camera*camera,float x, float y){//x,y {-1~+1}
//ray_center = ((cam_center - (cam_x * x) - (cam_y * y)) - cam_pos)
Vector3D center= vector3d_sub(vector3d_sub(camera->m_center,vector3d_mul_float(camera->x,x)),vector3d_mul_float(camera->y,y));
return ray_new(camera->m_pos,vector3d_normalize(vector3d_sub(center,camera->m_pos)));
}
/* accelerations in g (scale does not matter because values will be normolized),
* angular rates in rad/s (scale is MATTER),
* magnetic flux in uT (scale does not matter because values will be normolized),
* time in s */
void dcmUpdate(float *acc, float *gyro, float *mag, float imu_interval){
uint32_t i = 0;
float Kacc[3]; //K(b) vector according to accelerometer in body's coordinates
float Imag[3]; //I(b) vector accordng to magnetometer in body's coordinates
float w[3]; //gyro rates (angular velocity of a global vector in local coordinates)
float wA[3]; //correction vector to bring dcmEst's K vector closer to Acc
float wM[3]; //correction vector to bring dcmEst's I vector closer
//interval since last call
//imu_interval_ms = itg3200_period;
//---------------
// I,J,K unity vectors of global coordinate system I-North,J-West,K-zenith
// i,j,k unity vectors of body's coordiante system i-"nose", j-"left wing", k-"top"
//---------------
// [I.i , I.j, I.k]
// DCM = [J.i , J.j, J.k]
// [K.i , K.j, K.k]
//---------------
//Acelerometer
//---------------
/* Accelerometer measures gravity vector G in body coordinate system
Gravity vector is the reverse of K unity vector of global system
expressed in local coordinates K vector coincides with the z
coordinate of body's i,j,k vectors expressed in global
coordinates (K.i , K.j, K.k)
Acc can estimate global K vector(zenith) measured in body's coordinate
systems (the reverse of gravitation vector) */
// Kacc[0] = -xacc;
// Kacc[1] = -yacc;
// Kacc[2] = -zacc;
/* Поскольку на вход функции мы подаем значения измеренных ускорений по осям,
а не вектор гравитации, то ничего инвертировать не надо.
Вектор кажущегося ускорения совпадает с зенитным вектором К. */
for (i = 0; i<3; i++)
Kacc[i] = acc[i];
nue2nwu(Kacc);
vector3d_normalize(Kacc);
// calculate correction vector to bring dcmEst's K vector closer to Acc
// vector (K vector according to accelerometer)
// wA = Kgyro x Kacc , rotation needed to bring Kacc to Kgyro
vector3d_cross(dcmEst[2], Kacc, wA);
//---------------
//Magnetomer
//---------------
// calculate correction vector to bring dcmEst's I vector closer
// to Mag vector (I vector according to magnetometer)
// in the absense of magnetometer let's assume North vector (I) is
// always in XZ plane of the device (y coordinate is 0)
// Imag[0] = sqrtf(1 - dcmEst[0][2] * dcmEst[0][2]);
// Imag[1] = 0;
// Imag[2] = dcmEst[0][2];
for (i = 0; i<3; i++)
Imag[i] = mag[i];
nue2nwu(Imag);
/* Проработать комплексирование с нижним рядом DCM вместо вектора
* гравитации. Какие-то непонятные результаты получаются, или я их
* готовить не умею. */
float tmpM[3];
vector3d_normalize(Imag);
//vector3d_cross(Kacc, Imag, tmpM);
vector3d_cross(dcmEst[2], Imag, tmpM);
vector3d_normalize(tmpM);
//vector3d_cross(tmpM, Kacc, Imag);
vector3d_cross(tmpM, dcmEst[2], Imag);
vector3d_normalize(Imag);
// wM = Igyro x Imag, roation needed to bring Imag to Igyro
vector3d_cross(dcmEst[0], Imag, wM);
//---------------
//dcmEst
//---------------
//gyro rate direction is usually specified (in datasheets) as the device's(body's) rotation
//about a fixed earth's (global) frame, if we look from the perspective of device then
//the global vectors (I,K,J) rotation direction will be the inverse
//rotation rate about accelerometer's X axis (GY output)
//rotation rate about accelerometer's Y axis (GX output)
//rotation rate about accelerometer's Z axis (GZ output)
for (i = 0; i<3; i++)
w[i] = -gyro[i];
nue2nwu(w);
/* correction factors for accelerometer and mognetometer weights */
float magcor, acccor;
if (1 == GlobalFlags.gyro_cal){
acccor = 10;
magcor = 10;
}
else{
acccor = 1;
magcor = 1;
}
float aw = *accweight * acccor;
float mw = *magweight * magcor;
for(i=0; i<3; i++){
w[i] *= imu_interval; //scale by elapsed time to get angle in radians
//compute weighted average with the accelerometer correction vector
w[i] = (w[i] + aw * wA[i] + mw * wM[i]) /
(1.0f + aw + mw);
}
dcm_rotate(dcmEst, w);
imu_step++;
}
