I am programming a new software deck which will work using some stabilizer data, namely - sensorData, control, and motor. However, I am not being able to access these data. I have written a deck which has following content under the while(1) loop:
Code: Select all
while(1){
// DEBUG_PRINT("in_value: %f, %f, %f, %f, %f, %f\n", sensorData.gyro.x/150, sensorData.gyro.y/150, sensorData.gyro.z/150, sensorData.acc.x/150, sensorData.acc.y/150, sensorData.acc.z/150);
tick++;
// Error model input preparation and evaluate
// Saving old inputs
DEBUG_PRINT("mod: %d\n",tick % 100);
if (tick % 100 ==0){
sensorsAcquire(&sensorData, tick);
for (int ii=0;ii<6;ii++){
inModelIMUOld[ii] = inModelIMU[ii];
}
inModelIMU[0] = sensorData.gyro.x/150;
inModelIMU[1] = sensorData.gyro.y/150;
inModelIMU[2] = sensorData.gyro.z/150;
inModelIMU[3] = sensorData.acc.x/150;
inModelIMU[4] = sensorData.acc.y/150;
inModelIMU[5] = sensorData.acc.z/150;
for (int ii=0;ii<6;ii++){
// Preparing whole input for model
inModelIMU[6+ii] = inModelIMUOld[ii];
outModelIMU[ii] = 0;
}
// DEBUG_PRINT("tick: %d\n",tick);
DEBUG_PRINT("in_value: %f, %f, %f, %f, %f, %f\n", sensorData.gyro.x/150, sensorData.gyro.y/150, sensorData.gyro.z/150, sensorData.acc.x/150, sensorData.acc.y/150, sensorData.acc.z/150);
DEBUG_PRINT("in_value: %f, %f, %f, %f, %f, %f\n", inModelIMU[0], inModelIMU[1], inModelIMU[2], inModelIMU[3], inModelIMU[4], inModelIMU[5]);
get_inference_IMU(&inModelIMU,12/2,&outModelIMU);
// vTaskDelay(M2T(100));
DEBUG_PRINT("ou_value: %f, %f, %f, %f, %f, %f\n", outModelIMU[0], outModelIMU[1], outModelIMU[2], outModelIMU[3], outModelIMU[4], outModelIMU[5]);
errorModelIMU[0] = outModelIMU[0] - sensorData.gyro.x/150;
errorModelIMU[1] = outModelIMU[1] - sensorData.gyro.y/150;
errorModelIMU[2] = outModelIMU[2] - sensorData.gyro.z/150;
errorModelIMU[3] = outModelIMU[3] - sensorData.acc.x/150;
errorModelIMU[4] = outModelIMU[4] - sensorData.acc.y/150;
errorModelIMU[5] = outModelIMU[5] - sensorData.acc.z/150;
}
// vTaskDelay(M2T(100));
// DEBUG_PRINT("tick: %d\n",tick);
}