static void mat_mul_demo(void)
{
while (1) {
void *data;
int len;
/* wait for data... */
buffer_pull(&data, &len);
/* Process incoming message/data */
if (*(int *)data == SHUTDOWN_MSG) {
/* disable interrupts and free resources */
remoteproc_resource_deinit(proc);
break;
} else {
env_memcpy(matrix_array, data, len);
/* Process received data and multiple matrices. */
Matrix_Multiply(&matrix_array[0], &matrix_array[1], &matrix_result);
/* Send result back */
if (RPMSG_SUCCESS != rpmsg_send(app_rp_chnl, &matrix_result, sizeof(matrix))) {
xil_printf("Error: rpmsg_send failed\n");
}
}
}
}
void Matrix_update(float dt)
{
Vector_Add(&Omega[0], &ahrs_dcm.imu_rate.p, &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
#if OUTPUTMODE==1 // With corrected data (drift correction)
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -dt * Omega_Vector[2]; //-z
Update_Matrix[0][2] = dt * Omega_Vector[1]; //y
Update_Matrix[1][0] = dt * Omega_Vector[2]; //z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -dt * Omega_Vector[0]; //-x
Update_Matrix[2][0] = -dt * Omega_Vector[1]; //-y
Update_Matrix[2][1] = dt * Omega_Vector[0]; //x
Update_Matrix[2][2] = 0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -dt * ahrs_dcm.imu_rate.r; //-z
Update_Matrix[0][2] = dt * ahrs_dcm.imu_rate.q; //y
Update_Matrix[1][0] = dt * ahrs_dcm.imu_rate.r; //z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -dt * ahrs_dcm.imu_rate.p;
Update_Matrix[2][0] = -dt * ahrs_dcm.imu_rate.q;
Update_Matrix[2][1] = dt * ahrs_dcm.imu_rate.p;
Update_Matrix[2][2] = 0;
#endif
Matrix_Multiply(DCM_Matrix, Update_Matrix, Temporary_Matrix); //a*b=c
for (int x = 0; x < 3; x++) { //Matrix Addition (update)
for (int y = 0; y < 3; y++) {
DCM_Matrix[x][y] += Temporary_Matrix[x][y];
}
}
}
void IRenderPipeline3D::VertexShader(Vertex& inVertex)
{
VertexShaderOutput_Vertex outVertex;
//vertex position
VECTOR4 pos(inVertex.pos.x, inVertex.pos.y, inVertex.pos.z, 1.0f);
pos = Matrix_Multiply(mWorldMatrix, pos);
pos = Matrix_Multiply(mViewMatrix, pos);
//non-linear operation
float Z_ViewSpace = pos.z;
pos = Matrix_Multiply(mProjMatrix, pos);
if (Z_ViewSpace >= 0)
{
pos.x /= (Z_ViewSpace);
pos.y /= (Z_ViewSpace);
}
outVertex.posH = pos;
//Normal also need transformation,actually it need a World-Inverse-Transpose
//But I rule out all non-Orthogonal Transformation, so inverse can just use Transpose to substitude
//thus inverse-transpose yields world matrix itself(without translation)
MATRIX4x4 WorldMat_NoTrans = mWorldMatrix;
WorldMat_NoTrans.SetColumn(3, { 0,0,0,1.0f });
VECTOR4 normW(inVertex.normal.x, inVertex.normal.y, inVertex.normal.z, 1.0f);
normW = Matrix_Multiply(WorldMat_NoTrans, normW);
//-----TexCoord
outVertex.texcoord = VECTOR2(
inVertex.texcoord.x*mTexCoord_scale + mTexCoord_offsetX,
inVertex.texcoord.y*mTexCoord_scale + mTexCoord_offsetY);
//...Vertex Light or directly use vertex color
if (mLightEnabled)
{
outVertex.color = mFunction_VertexLighting(inVertex.pos, VECTOR3(normW.x,normW.y,normW.z));
}
else
{
outVertex.color = inVertex.color;
}
m_pVB_HomoSpace->push_back(outVertex);
}
void Matrix_update(void)
{
int diff=0;
for(int axis=0; axis <3; axis++){
if(SENSOR_SIGN[axis]<0){
diff = AN_OFFSET[axis]-AN[axis];
Gyro_Vector[axis]=Gyro_Scaled(diff); //gyro x roll
//Gyro_Vector[axis]=(AN_OFFSET[axis]-AN[axis]); //gyro x roll
}
else{
diff = AN[axis] - AN_OFFSET[axis];
Gyro_Vector[axis]=Gyro_Scaled(diff); //gyro x roll
//Gyro_Vector[axis]=Gyro_Scaled_X(AN[axis]-AN_OFFSET[axis]); //gyro x roll
}
}
Accel_Vector[0]=accel_x;
Accel_Vector[1]=accel_y;
Accel_Vector[2]=accel_z;
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
//Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement
#if OUTPUTMODE==1
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#endif
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
for(int x=0; x<3; x++) //Matrix Addition (update)
{
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
MATRIX4x4 Math::Matrix_YawPitchRoll(float yaw_Y, float pitch_X, float roll_Z)
{
//rotation of Cardan Angle. If use column vector, then the
//matrix will be generated by [Ry] x [Rx] x[Rz]
MATRIX4x4 matRotateX;
MATRIX4x4 matRotateY;
MATRIX4x4 matRotateZ;
matRotateX = Matrix_RotationX(pitch_X);
matRotateY = Matrix_RotationY(yaw_Y);
matRotateZ = Matrix_RotationZ(roll_Z);
//outMatrix = [M_RY] x [M_RX] x [M_RZ]
//(a column vector can be pre-Multiplied by this matrix)
MATRIX4x4 outMatrix;
outMatrix = Matrix_Multiply(matRotateY, Matrix_Multiply(matRotateX, matRotateZ));
return outMatrix;
}
void update_rotation_matrix(float phi, float theta, float psi)
{
// voir doc (QuadConv p7)
// phi
float cos_phi = cos(phi); // optimisations
float sin_phi = sin(phi);
// les matrices des rotations sont transposées car la rotation
// de RT par rapport à RQ est inverse de RQ par rapport à RT
fMatrix_t Mphi = {
{cos_phi, 0, sin_phi},
{0, 1, 0},
{-sin_phi, 0, cos_phi}
};
// theta
float cos_theta = cos(theta);
float sin_theta = sin(theta);
fMatrix_t Mtheta = {
{1, 0, 0},
{0, cos_theta, sin_theta},
{0, -sin_theta, cos_theta}
};
// psi
float cos_psi = cos(psi);
float sin_psi = sin(psi);
fMatrix_t Mpsi = {
{cos_psi, -sin_psi, 0},
{sin_psi, cos_psi, 0},
{0, 0, 1}
};
// matrice temporraire
fMatrix_t Mtemp;
// ordre de multiplication tranposé : (Mphi * Mtheta * Mpsi)T = (Mpsi T * Mtheta T) * Mphi T
// eq.2
Matrix_Multiply(Mpsi, Mtheta, Mtemp);
Matrix_Multiply(Mtemp, Mphi, RotMat);
}
static void mat_mul_demo(void *unused_arg)
{
int status = 0;
(void)unused_arg;
/* Create buffer to send data between RPMSG callback and processing task */
buffer_create();
/* Initialize HW and SW components/objects */
init_system();
/* Resource table needs to be provided to remoteproc_resource_init() */
rsc_info.rsc_tab = (struct resource_table *)&resources;
rsc_info.size = sizeof(resources);
/* Initialize OpenAMP framework */
status = remoteproc_resource_init(&rsc_info, rpmsg_channel_created,
rpmsg_channel_deleted, rpmsg_read_cb,
&proc);
if (RPROC_SUCCESS != status) {
xil_printf("Error: initializing OpenAMP framework\n");
return;
}
/* Stay in data processing loop until we receive a 'shutdown' message */
while (1) {
void *data;
int len;
/* wait for data... */
buffer_pull(&data, &len);
/* Process incoming message/data */
if (*(int *)data == SHUTDOWN_MSG) {
/* disable interrupts and free resources */
remoteproc_resource_deinit(proc);
/* Terminate this task */
vTaskDelete(NULL);
break;
} else {
env_memcpy(matrix_array, data, len);
/* Process received data and multiple matrices. */
Matrix_Multiply(&matrix_array[0], &matrix_array[1], &matrix_result);
/* Send result back */
if (RPMSG_SUCCESS != rpmsg_send(app_rp_chnl, &matrix_result, sizeof(matrix))) {
xil_printf("Error: rpmsg_send failed\n");
}
}
}
}
void Matrix_update(void)
{
// maybe make the values EXTERN to reduce function calls
Gyro_Vector[0] = ToRad(((float)imu_read_sensor(GYRO_X)/ GYRO_DIV)); //gyro x roll // 14.375 = Gyro RAW to Grad/s
Gyro_Vector[1] = ToRad(((float)imu_read_sensor(GYRO_Y)/ GYRO_DIV)); //gyro y pitch
Gyro_Vector[2] = ToRad(((float)imu_read_sensor(GYRO_Z)/ GYRO_DIV)); //gyro Z yaw
Accel_Vector[0] = imu_read_sensor(ACC_X);
Accel_Vector[1] = imu_read_sensor(ACC_Y);
Accel_Vector[2] = imu_read_sensor(ACC_Z); // Expect 0,0,4096 (ideal)
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
//Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement
#if OUTPUTMODE==1
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -G_Dt * Omega_Vector[2];//-z
Update_Matrix[0][2] = G_Dt * Omega_Vector[1];//y
Update_Matrix[1][0] = G_Dt * Omega_Vector[2];//z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -G_Dt * Omega_Vector[0];//-x
Update_Matrix[2][0] = -G_Dt * Omega_Vector[1];//-y
Update_Matrix[2][1] = G_Dt * Omega_Vector[0];//x
Update_Matrix[2][2] = 0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -G_Dt * Gyro_Vector[2];//-z
Update_Matrix[0][2] = G_Dt * Gyro_Vector[1];//y
Update_Matrix[1][0] = G_Dt * Gyro_Vector[2];//z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -G_Dt * Gyro_Vector[0];
Update_Matrix[2][0] = -G_Dt * Gyro_Vector[1];
Update_Matrix[2][1] = G_Dt * Gyro_Vector[0];
Update_Matrix[2][2] = 0;
#endif
Matrix_Multiply(&DCM_Matrix[0][0], &Update_Matrix[0][0],
&Temporary_Matrix[0][0]); //a*b=c
int x, y;
for (x = 0; x < 3; x++) //Matrix Addition (update)
{
for (y = 0; y < 3; y++)
{
DCM_Matrix[x][y] += Temporary_Matrix[x][y];
}
}
}
void Matrix_update(void)
{
Gyro_Vector[0] = Gyro_Scaled_X(gyro_x); //gyro x roll
Gyro_Vector[1] = Gyro_Scaled_Y(gyro_y); //gyro y pitch
Gyro_Vector[2] = Gyro_Scaled_Z(gyro_z); //gyro Z yaw
Accel_Vector[0] = accel_x;
Accel_Vector[1] = accel_y;
Accel_Vector[2] = accel_z;
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
//Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement
#if OUTPUTMODE==1
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -G_Dt * Omega_Vector[2];//-z
Update_Matrix[0][2] = G_Dt * Omega_Vector[1];//y
Update_Matrix[1][0] = G_Dt * Omega_Vector[2];//z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -G_Dt * Omega_Vector[0];//-x
Update_Matrix[2][0] = -G_Dt * Omega_Vector[1];//-y
Update_Matrix[2][1] = G_Dt * Omega_Vector[0];//x
Update_Matrix[2][2] = 0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#endif
Matrix_Multiply(DCM_Matrix, Update_Matrix, Temporary_Matrix); //a*b=c
int x;
for (x = 0; x < 3; x++) //Matrix Addition (update)
{
int y;
for (y = 0; y < 3; y++)
{
DCM_Matrix[x][y] += Temporary_Matrix[x][y];
}
}
}
static void rpmsg_read_cb(struct rpmsg_channel *rp_chnl, void *data, int len,
void *priv, unsigned long src)
{
if ((*(int *)data) == SHUTDOWN_MSG) {
remoteproc_resource_deinit(proc);
} else {
env_memcpy(matrix_array, data, len);
/* Process received data and multiple matrices. */
Matrix_Multiply(&matrix_array[0], &matrix_array[1],
&matrix_result);
/* Send the result of matrix multiplication back to master. */
rpmsg_send(app_rp_chnl, &matrix_result, sizeof(matrix));
}
}
void matrix_mul(){
#ifdef USE_FREERTOS
for( ;; ){
if( xQueueReceive(mat_mul_queue, &mat_array, portMAX_DELAY )){
env_memcpy(matrix_array, mat_array.data, mat_array.length);
Matrix_Multiply(&matrix_array[0], &matrix_array[1], &matrix_result);
mat_result_array.length = sizeof(matrix);
mat_result_array.data = &matrix_result;
xQueueSend( OpenAMPInstPtr.send_queue, &mat_result_array, portMAX_DELAY );
}
}
#else
if(pq_qlength(mat_mul_queue) > 0){
mat_array = pq_dequeue(mat_mul_queue);
env_memcpy(matrix_array, mat_array->data, mat_array->length);
/* Process received data and multiple matrices. */
Matrix_Multiply(&matrix_array[0], &matrix_array[1], &matrix_result);
mat_result_array.length = sizeof(matrix);
mat_result_array.data = &matrix_result;
pq_enqueue(OpenAMPInstPtr.send_queue, &mat_result_array);
}
#endif
}
void Matrix_update(void)
{
Gyro_Vector[0] = gyro_scale(g[X]); //gyro x roll
Gyro_Vector[1] = gyro_scale(g[Y]); //gyro y pitch
Gyro_Vector[2] = gyro_scale(g[Z]); //gyro Z yaw
Accel_Vector[0] = -a[X];
Accel_Vector[1] = -a[Y];
Accel_Vector[2] = -a[Z];
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
//Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement
#ifdef CORRECT_DRIFT
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#endif
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
for(int x=0; x<3; x++) //Matrix Addition (update)
{
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
void DCM_matrixUpdate(float timeDiff, float* gyroRadSec)
{
int x = 0, y = 0;
Vector_Add(&Omega[0], &gyroRadSec[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
// Update_Matrix[0][0]=0;
// Update_Matrix[0][1]=-timeDiff*gyroRadSec[2];//-z
// Update_Matrix[0][2]=timeDiff*gyroRadSec[1];//y
// Update_Matrix[1][0]=timeDiff*gyroRadSec[2];//z
// Update_Matrix[1][1]=0;
// Update_Matrix[1][2]=-timeDiff*gyroRadSec[0];//-x
// Update_Matrix[2][0]=-timeDiff*gyroRadSec[1];//-y
// Update_Matrix[2][1]=timeDiff*gyroRadSec[0];//x
// Update_Matrix[2][2]=0;
//
//drift correction
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-timeDiff*Omega_Vector[2];//-z
Update_Matrix[0][2]=timeDiff*Omega_Vector[1];//y
Update_Matrix[1][0]=timeDiff*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-timeDiff*Omega_Vector[0];//-x
Update_Matrix[2][0]=-timeDiff*Omega_Vector[1];//-y
Update_Matrix[2][1]=timeDiff*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
Matrix_Multiply(Temporary_Matrix, DCM_Matrix,Update_Matrix); //a*b=c
while (x < 3)
{
y= 0;
while (y < 3)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
y++;
}
x++;
}
}
void Matrix_update(void)
{
Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
Accel_Vector[0]=accel[0];
Accel_Vector[1]=accel[1];
Accel_Vector[2]=accel[2];
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
#if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#else // Use drift correction
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
#endif
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
int x,y;
for(x=0; x<3; x++) //Matrix Addition (update)
{
for(y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
void Matrix_update( int gx , int gy , int gz , int ax , int ay , int az )
{
Gyro_Vector[0] = Gyro_Scaled_X(fix16_from_int(-gx));
Gyro_Vector[1] = Gyro_Scaled_X(fix16_from_int(gy));
Gyro_Vector[2] = Gyro_Scaled_X(fix16_from_int(-gz));
/*
Accel_Vector[0]=fix16_sadd(fix16_mul(Accel_Vector[0],const_fix16_from_dbl(0.6)),
fix16_mul(fix16_from_int(ax),const_fix16_from_dbl(0.4)));
Accel_Vector[1]=fix16_sadd(fix16_mul(Accel_Vector[0],const_fix16_from_dbl(0.6)),
fix16_mul(fix16_from_int(ay),const_fix16_from_dbl(0.4)));
Accel_Vector[2]=fix16_sadd(fix16_mul(Accel_Vector[0],const_fix16_from_dbl(0.6)),
fix16_mul(fix16_from_int(az),const_fix16_from_dbl(0.4)));
*/
/* NO LOW PASS FILTER */
Accel_Vector[0] = fix16_from_int(ax);
Accel_Vector[1] = fix16_from_int(-ay);
Accel_Vector[2] = fix16_from_int(az);
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional
#if OUTPUTMODE==1 // corrected mode
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -fix16_mul(G_Dt, Omega_Vector[2]);//-z
Update_Matrix[0][2] = fix16_mul(G_Dt, Omega_Vector[1]);//y
Update_Matrix[1][0] = fix16_mul(G_Dt, Omega_Vector[2]);//z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -fix16_mul(G_Dt, Omega_Vector[0]);//-x
Update_Matrix[2][0] = -fix16_mul(G_Dt, Omega_Vector[1]);//-y
Update_Matrix[2][1] = fix16_mul(G_Dt, Omega_Vector[0]);//x
Update_Matrix[2][2] = 0;
#endif
Matrix_Multiply(DCM_Matrix, Update_Matrix, Temporary_Matrix);
Matrix_Addto(DCM_Matrix, Temporary_Matrix);
}
//input Confo_Angle (*dist,*bend,*tort), output XYZ_Type data_structure (*r)
//dist[0] : -1.0;
//dist[1]-[n]: normal
//bend[0]-[1]: -9.9
//bend[2]-[n]: normal
//tort[0]-[2]: -9.9
//tort[3]-[n]: normal
void Confo_Lett::ctc_ori(double *dist,double *bend,double *tort,int n,XYZ *r,XYZ *init)
{
int i;
double radi;
XYZ xyz;
xyz=0.0;
XYZ ori[3],pos[3];
//real_process//
Matrix_Unit(cle_T_Back,1.0);
for(i=0;i<n;i++)
{
if(dist==NULL)radi=3.8;
else radi=dist[i];
if(i==0)r[i]=0.0;
else if(i==1)
{
r[i]=0.0;
r[i].X=radi;
}
else if(i==2)
{
Universal_Rotation(cle_Z_Axis,bend[i],cle_R_Theta);
Vector_Dot(cle_D_Back,radi,cle_X_Axis,3);
Matrix_Multiply(cle_T_Pre,cle_T_Back,cle_R_Theta);
Vector_Multiply(cle_D_Pre,cle_T_Pre,cle_D_Back);
Matrix_Equal(cle_T_Back,cle_T_Pre);
xyz.double2xyz(cle_D_Pre);
r[i]=r[i-1]+xyz;
}
else
{
Universal_Rotation(cle_Z_Axis,bend[i],cle_R_Theta);
Universal_Rotation(cle_X_Axis,tort[i],cle_R_Thor);
Vector_Dot(cle_D_Back,radi,cle_X_Axis,3);
Matrix_Multiply(cle_temp,cle_R_Thor,cle_R_Theta);
Matrix_Multiply(cle_T_Pre,cle_T_Back,cle_temp);
Vector_Multiply(cle_D_Pre,cle_T_Pre,cle_D_Back);
Matrix_Equal(cle_T_Back,cle_T_Pre);
xyz.double2xyz(cle_D_Pre);
r[i]=r[i-1]+xyz;
}
}
//whether do ini_vector
if(init!=NULL && n>=3)
{
for(i=0;i<3;i++)
{
ori[i]=init[i];
pos[i]=r[i];
}
Get_Initial(ori,pos,cle_r1,cle_r2);
Matrix_Multiply(cle_T_Pre,cle_r2,cle_r1);
for(i=0;i<n;i++)
{
r[i].xyz2double(cle_D_Back);
Vector_Multiply(cle_D_Pre,cle_T_Pre,cle_D_Back);
xyz.double2xyz(cle_D_Pre);
r[i]=ori[0]+xyz;
}
}
}
/* Application entry point */
int main() {
int status;
struct remote_proc *proc;
int i;
int shutdown_msg = SHUTDOWN_MSG;
/* Switch to System Mode */
SWITCH_TO_SYS_MODE();
/* Initialize HW system components */
init_system();
status = remoteproc_init((void *) fw_name, rpmsg_channel_created, rpmsg_channel_deleted, rpmsg_read_cb, &proc);
if(!status)
{
status = remoteproc_boot(proc);
}
if(status)
{
return -1;
}
while (1) {
if (int_flag) {
if(shutdown_called == 1)
{
break;
}
/* Process received data and multiple matrices. */
Matrix_Multiply(&matrix_array[0], &matrix_array[1], &matrix_result);
/* Send the result of matrix multiplication back to master. */
rpmsg_send(app_rp_chnl, &matrix_result, sizeof(matrix));
int_flag = 0;
sleep();
}
sleep();
}
/* Send shutdown message to remote */
rpmsg_send(app_rp_chnl, &shutdown_msg, sizeof(int));
for (i = 0; i < 100000; i++)
{
sleep();
}
remoteproc_shutdown(proc);
remoteproc_deinit(proc);
return 0;
}
/* headingKalman
*
* Implements a 1-dimensional, 1st order Kalman Filter
*
* That is, it deals with heading and heading rate (h and h') but no other
* state variables. The state equations are:
*
* X = A X^
* h = h + h'dt --> | h | = | 1 dt | | h |
* h' = h' | h' | | 0 1 | | h' |
*
* Kalman Filtering is not that hard. If it's hard you haven't found the right
* teacher. Try taking CS373 from Udacity.com
*
* This notation is Octave (Matlab) syntax and is based on the Bishop-Welch
* paper and references the equation numbers in that paper.
* http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
*
* returns : current heading estimate
*/
float headingKalman(float dt, float Hgps, bool gps, float dHgyro, bool gyro)
{
A[1] = dt;
/* Initialize, first time thru
x = H*z0
*/
//fprintf(stdout, "gyro? %c gps? %c\n", (gyro)?'Y':'N', (gps)?'Y':'N');
// Depending on what sensor measurements we've gotten,
// switch between observer (H) matrices and measurement noise (R) matrices
// TODO 3 incorporate HDOP or sat count in R
if (gps) {
H[0] = 1.0;
z[0] = Hgps;
} else {
H[0] = 0;
z[0] = 0;
}
if (gyro) {
H[3] = 1.0;
z[1] = dHgyro;
} else {
H[3] = 0;
z[1] = 0;
}
//Matrix_print(2,2, A, "1. A");
//Matrix_print(2,2, P, " P");
//Matrix_print(2,1, x, " x");
//Matrix_print(2,1, K, " K");
//Matrix_print(2,2, H, "2. H");
//Matrix_print(2,1, z, " z");
/**********************************************************************
* Predict
%
* In this step we "move" our state estimate according to the equation
*
* x = A*x; // Eq 1.9
***********************************************************************/
float xp[2];
Matrix_Multiply(2,2,1, xp, A, x);
//Matrix_print(2,1, xp, "3. xp");
/**********************************************************************
* We also have to "move" our uncertainty and add noise. Whenever we move,
* we lose certainty because of system noise.
*
* P = A*P*A' + Q; // Eq 1.10
***********************************************************************/
float At[4];
Matrix_Transpose(2,2, At, A);
float AP[4];
Matrix_Multiply(2,2,2, AP, A, P);
float APAt[4];
Matrix_Multiply(2,2,2, APAt, AP, At);
Matrix_Add(2,2, P, APAt, Q);
//Matrix_print(2,2, P, "4. P");
/**********************************************************************
* Measurement aka Correct
* First, we have to figure out the Kalman Gain which is basically how
* much we trust the sensor measurement versus our prediction.
*
* K = P*H'*inv(H*P*H' + R); // Eq 1.11
***********************************************************************/
float Ht[4];
//Matrix_print(2,2, H, "5. H");
Matrix_Transpose(2,2, Ht, H);
//Matrix_print(2,2, Ht, "5. Ht");
float HP[2];
//Matrix_print(2,2, P, "5. P");
Matrix_Multiply(2,2,2, HP, H, P);
//Matrix_print(2,2, HP, "5. HP");
float HPHt[4];
Matrix_Multiply(2,2,2, HPHt, HP, Ht);
//Matrix_print(2,2, HPHt, "5. HPHt");
float HPHtR[4];
//Matrix_print(2,2, R, "5. R");
Matrix_Add(2,2, HPHtR, HPHt, R);
//Matrix_print(2,2, HPHtR, "5. HPHtR");
Matrix_Inverse(2, HPHtR);
//Matrix_print(2,2, HPHtR, "5. HPHtR");
float PHt[2];
//Matrix_print(2,2, P, "5. P");
//Matrix_print(2,2, Ht, "5. Ht");
Matrix_Multiply(2,2,2, PHt, P, Ht);
//Matrix_print(2,2, PHt, "5. PHt");
Matrix_Multiply(2,2,2, K, PHt, HPHtR);
//Matrix_print(2,2, K, "5. K");
/**********************************************************************
* Then we determine the discrepancy between prediction and measurement
* with the "Innovation" or Residual: z-H*x, multiply that by the
* Kalman gain to correct the estimate towards the prediction a little
* at a time.
*
* x = x + K*(z-H*x); // Eq 1.12
***********************************************************************/
float Hx[2];
Matrix_Multiply(2,2,1, Hx, H, xp);
//Matrix_print(2,2, H, "6. H");
//Matrix_print(2,1, x, "6. x");
//Matrix_print(2,1, Hx, "6. Hx");
float zHx[2];
Matrix_Subtract(2,1, zHx, z, Hx);
zHx[0] = clamp180(zHx[0]);
//Matrix_print(2,1, z, "6. z");
//Matrix_print(2,1, zHx, "6. zHx");
float KzHx[2];
Matrix_Multiply(2,2,1, KzHx, K, zHx);
//Matrix_print(2,2, K, "6. K");
//Matrix_print(2,1, KzHx, "6. KzHx");
Matrix_Add(2,1, x, xp, KzHx);
x[0] = clamp360(x[0]); // Clamp to 0-360 range
//Matrix_print(2,1, x, "6. x");
/**********************************************************************
* We also have to adjust the certainty. With a new measurement, the
* estimate certainty always increases.
*
* P = (I-K*H)*P; // Eq 1.13
***********************************************************************/
float KH[4];
//Matrix_print(2,2, K, "7. K");
Matrix_Multiply(2,2,2, KH, K, H);
//Matrix_print(2,2, KH, "7. KH");
float IKH[4];
Matrix_Subtract(2,2, IKH, I, KH);
//Matrix_print(2,2, IKH, "7. IKH");
float P2[4];
Matrix_Multiply(2,2,2, P2, IKH, P);
Matrix_Copy(2, 2, P, P2);
//Matrix_print(2,2, P, "7. P");
return x[0];
}
ERet DeferredRenderer::RenderScene( const SceneView& sceneView, const Clump& sceneData )
{
gfxMARKER(RenderScene);
if( sceneView.viewportWidth != m_viewportWidth || sceneView.viewportHeight != m_viewportHeight )
{
this->ResizeBuffers( sceneView.viewportWidth, sceneView.viewportHeight, false );
m_viewportWidth = sceneView.viewportWidth;
m_viewportHeight = sceneView.viewportHeight;
}
mxDO(BeginRender_GBuffer());
G_PerCamera cbPerView;
{
cbPerView.g_viewMatrix = sceneView.viewMatrix;
cbPerView.g_viewProjectionMatrix = sceneView.viewMatrix * sceneView.projectionMatrix;
cbPerView.g_inverseViewMatrix = Matrix_OrthoInverse( sceneView.viewMatrix );
cbPerView.g_projectionMatrix = sceneView.projectionMatrix;
cbPerView.g_inverseProjectionMatrix = ProjectionMatrix_Inverse( sceneView.projectionMatrix );
cbPerView.g_inverseViewProjectionMatrix = Matrix_Inverse( cbPerView.g_viewProjectionMatrix );
cbPerView.g_WorldSpaceCameraPos = sceneView.worldSpaceCameraPos;
float n = sceneView.nearClip;
float f = sceneView.farClip;
float x = 1 - f / n;
float y = f / n;
float z = x / f;
float w = y / f;
cbPerView.g_ZBufferParams = Float4_Set( x, y, z, w );
cbPerView.g_ZBufferParams2 = Float4_Set( n, f, 1.0f/n, 1.0f/f );
float H = sceneView.projectionMatrix[0][0];
float V = sceneView.projectionMatrix[2][1];
float A = sceneView.projectionMatrix[1][2];
float B = sceneView.projectionMatrix[3][2];
cbPerView.g_ProjParams = Float4_Set( H, V, A, B );
// x = 1/H
// y = 1/V
// z = 1/B
// w = -A/B
cbPerView.g_ProjParams2 = Float4_Set( 1/H, 1/V, 1/B, -A/B );
llgl::UpdateBuffer(llgl::GetMainContext(), m_hCBPerCamera, sizeof(cbPerView), &cbPerView);
}
// G-Buffer Stage: Render all solid objects to a very sparse G-Buffer
{
gfxMARKER(Fill_Geometry_Buffer);
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "Default"));
G_PerObject cbPerObject;
TObjectIterator< rxModel > modelIt( sceneData );
while( modelIt.IsValid() )
{
const rxModel& model = modelIt.Value();
const Float3x4* TRS = model.m_transform;
{
cbPerObject.g_worldMatrix = Float3x4_Unpack( *TRS );
cbPerObject.g_worldViewMatrix = Matrix_Multiply(cbPerObject.g_worldMatrix, sceneView.viewMatrix);
cbPerObject.g_worldViewProjectionMatrix = Matrix_Multiply(cbPerObject.g_worldMatrix, sceneView.viewProjectionMatrix);
llgl::UpdateBuffer(m_hRenderContext, m_hCBPerObject, sizeof(cbPerObject), &cbPerObject);
}
const rxMesh* mesh = model.m_mesh;
for( int iSubMesh = 0; iSubMesh < mesh->m_parts.Num(); iSubMesh++ )
{
const rxSubmesh& submesh = mesh->m_parts[iSubMesh];
const rxMaterial* material = model.m_batches[iSubMesh];
const FxShader* shader = material->m_shader;
if( shader->localCBs.Num() )
{
mxASSERT(shader->localCBs.Num()==1);
const ParameterBuffer& uniforms = material->m_uniforms;
const FxCBuffer& rCB = shader->localCBs[0];
llgl::UpdateBuffer(m_hRenderContext, rCB.handle, uniforms.GetDataSize(), uniforms.ToPtr() );
}
llgl::DrawCall batch;
batch.Clear();
SetGlobalUniformBuffers( &batch );
BindMaterial( material, &batch );
batch.inputLayout = Rendering::g_inputLayouts[VTX_Draw];
batch.topology = mesh->m_topology;
batch.VB[0] = mesh->m_vertexBuffer;
batch.IB = mesh->m_indexBuffer;
batch.b32bit = (mesh->m_indexStride == sizeof(UINT32));
batch.baseVertex = submesh.baseVertex;
batch.vertexCount = submesh.vertexCount;
batch.startIndex = submesh.startIndex;
batch.indexCount = submesh.indexCount;
llgl::Submit(m_hRenderContext, batch);
}
modelIt.MoveToNext();
}
}
EndRender_GBuffer();
// Deferred Lighting Stage: Accumulate all lights as a screen space operation
{
gfxMARKER(Deferred_Lighting);
llgl::ViewState viewState;
{
viewState.Reset();
viewState.colorTargets[0].SetDefault();
viewState.targetCount = 1;
viewState.flags = llgl::ClearColor;
}
llgl::SubmitView(m_hRenderContext, viewState);
{
gfxMARKER(Directional_Lights);
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "Deferred_Lighting"));
FxShader* shader;
mxDO(GetByName(*m_rendererData, "deferred_directional_light", shader));
mxDO2(FxSlow_SetResource(shader, "GBufferTexture0", llgl::AsResource(m_colorRT0->handle), Rendering::g_samplers[PointSampler]));
mxDO2(FxSlow_SetResource(shader, "GBufferTexture1", llgl::AsResource(m_colorRT1->handle), Rendering::g_samplers[PointSampler]));
mxDO2(FxSlow_SetResource(shader, "DepthTexture", llgl::AsResource(m_depthRT->handle), Rendering::g_samplers[PointSampler]));
TObjectIterator< rxGlobalLight > lightIt( sceneData );
while( lightIt.IsValid() )
{
rxGlobalLight& light = lightIt.Value();
//mxDO(FxSlow_Commit(m_hRenderContext,shader));
DirectionalLight lightData;
{
lightData.direction = Matrix_TransformNormal(sceneView.viewMatrix, light.m_direction);
lightData.color = light.m_color;
}
mxDO2(FxSlow_UpdateUBO(m_hRenderContext,shader,"DATA",&lightData,sizeof(lightData)));
DrawFullScreenTriangle(shader);
lightIt.MoveToNext();
}
}
{
gfxMARKER(Point_Lights);
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "Deferred_Lighting"));
FxShader* shader;
mxDO(GetAsset(shader,MakeAssetID("deferred_point_light.shader"),m_rendererData));
mxDO2(FxSlow_SetResource(shader, "GBufferTexture0", llgl::AsResource(m_colorRT0->handle), Rendering::g_samplers[PointSampler]));
mxDO2(FxSlow_SetResource(shader, "GBufferTexture1", llgl::AsResource(m_colorRT1->handle), Rendering::g_samplers[PointSampler]));
mxDO2(FxSlow_SetResource(shader, "DepthTexture", llgl::AsResource(m_depthRT->handle), Rendering::g_samplers[PointSampler]));
TObjectIterator< rxLocalLight > lightIt( sceneData );
while( lightIt.IsValid() )
{
rxLocalLight& light = lightIt.Value();
//mxDO(FxSlow_Commit(m_hRenderContext,shader));
PointLight lightData;
{
Float3 viewSpaceLightPosition = Matrix_TransformPoint(sceneView.viewMatrix, light.position);
lightData.Position_InverseRadius = Float4_Set(viewSpaceLightPosition, 1.0f/light.radius);
lightData.Color_Radius = Float4_Set(light.color, light.radius);
}
mxDO2(FxSlow_UpdateUBO(m_hRenderContext,shader,"DATA",&lightData,sizeof(lightData)));
DrawFullScreenTriangle(shader);
lightIt.MoveToNext();
}
}
}
// Thursday, March 26, 2015 Implementing Weighted, Blended Order-Independent Transparency
//http://casual-effects.blogspot.ru/2015/03/implemented-weighted-blended-order.html
// Forward+ notes
//http://bioglaze.blogspot.fi/2014/07/2048-point-lights-60-fps.html
// Material Stage: Render all solid objects again while combining the lighting
// from Stage 2 with certain material-properties (colors, reflections, glow, fog, etc.)
// to produce the final image
#if 0
{
llgl::ViewState viewState;
{
viewState.Reset();
viewState.colorTargets[0].SetDefault();
viewState.targetCount = 1;
}
llgl::SubmitView(m_hRenderContext, viewState);
}
{
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "NoCulling"));
FxShader* shader;
mxDO(GetByName(*m_rendererData, "full_screen_triangle", shader));
mxDO(FxSlow_SetResource(shader, "t_sourceTexture", llgl::AsResource(pColorRT1->handle), Rendering::g_samplers[PointSampler]));
mxDO(FxSlow_Commit(m_hRenderContext,shader));
DrawFullScreenTriangle(shader);
}
#endif
#if 0
{
llgl::ViewState viewState;
{
viewState.Reset();
viewState.colorTargets[0].SetDefault();
viewState.targetCount = 1;
}
llgl::SubmitView(m_hRenderContext, viewState);
}
{
FxShader* shader;
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "Default"));
{
mxDO(GetAsset(shader,MakeAssetID("debug_draw_colored.shader"),m_rendererData));
mxDO(FxSlow_SetUniform(shader,"g_color",RGBAf::GRAY.ToPtr()));
mxDO(FxSlow_Commit(m_hRenderContext,shader));
DBG_Draw_Models_With_Custom_Shader(sceneView, sceneData, *shader);
}
mxDO(FxSlow_SetRenderState(m_hRenderContext, *m_rendererData, "NoCulling"));
{
mxDO(GetAsset(shader,MakeAssetID("debug_draw_normals.shader"),m_rendererData));
float lineLength = 4.0f;
mxDO(FxSlow_SetUniform(shader,"g_lineLength",&lineLength));
mxDO(FxSlow_Commit(m_hRenderContext,shader));
DBG_Draw_Models_With_Custom_Shader(sceneView, sceneData, *shader, Topology::PointList);
}
}
#endif
return ALL_OK;
}
