void SensorsDynamicCalibrate(char SensorType)
{
#if STACK_ACC
if(SensorType&SENSOR_ACC&&SensorInitState.ACC_Done)
{
/* TBD */
}
#endif
#if STACK_GYRO
if(SensorType&SENSOR_GYRO&&SensorInitState.GYRO_Done)
{
if(!SensorCalState.GYRO_Done)
{
if(nvtGyroCenterCalibrate()!=STATUS_GYRO_CAL_DONE) {}
//led_arm_state(LED_STATE_TOGGLE);
else
{
float GyroMean[3];
SensorCalState.GYRO_Done = true;
//led_arm_state(LED_STATE_OFF);
nvtGetGyroOffset(GyroMean);
}
}
}
#endif
#if STACK_MAG
if(SensorType&SENSOR_MAG&&SensorInitState.MAG_Done)
{
if(!SensorCalState.MAG_Done)
{
static float rpy[3],lastY,diff;
nvtGetEulerRPY(rpy);
diff = fabsf(rpy[2] - lastY);
if((diff>0.01f)||(diff==0)) {}
//led_mag_state(LED_STATE_TOGGLE);
else
{
//led_arm_state(LED_STATE_OFF);
SensorCalState.MAG_Done = true;
}
lastY = rpy[2];
}
}
#endif
}
void UpdateFlashCal(int8_t sensorType, bool erase)
{
uint8_t CalBase, i, QualityFactor;
float mean[3], scale[3],matrix[MAG_CAL_DATA_SIZE];
if(sensorType&SENSOR_GYRO) {
CalBase=CAL_BASE_GYRO;
nvtGetGyroOffset(mean);
nvtGetGyroScale(scale);
if(erase) {
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_INVALID));
return;
}
else
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_VALID));
DATA_FLASH_Write(CalBase++,float2dw(mean[0]));
DATA_FLASH_Write(CalBase++,float2dw(mean[1]));
DATA_FLASH_Write(CalBase++,float2dw(mean[2]));
DATA_FLASH_Write(CalBase++,float2dw(scale[0]));
DATA_FLASH_Write(CalBase++,float2dw(scale[1]));
DATA_FLASH_Write(CalBase++,float2dw(scale[2]));
printf("GyroMean.x:%f\n", mean[0]);
printf("GyroMean.y:%f\n", mean[1]);
printf("GyroMean.z:%f\n", mean[2]);
printf("GyroScale.x:%f\n", scale[0]);
printf("GyroScale.y:%f\n", scale[1]);
printf("GyroScale.z:%f\n", scale[2]);
}
if(sensorType&SENSOR_ACC) {
CalBase=CAL_BASE_ACC;
nvtGetAccOffset(mean);
nvtGetAccScale(scale);
if(erase) {
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_INVALID));
return;
}
else
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_VALID));
DATA_FLASH_Write(CalBase++,float2dw(mean[0]));
DATA_FLASH_Write(CalBase++,float2dw(mean[1]));
DATA_FLASH_Write(CalBase++,float2dw(mean[2]));
DATA_FLASH_Write(CalBase++,float2dw(scale[0]));
DATA_FLASH_Write(CalBase++,float2dw(scale[1]));
DATA_FLASH_Write(CalBase++,float2dw(scale[2]));
printf("AccMean.x:%f\n", mean[0]);
printf("AccMean.y:%f\n", mean[1]);
printf("AccMean.z:%f\n", mean[2]);
printf("AccScale.x:%f\n", scale[0]);
printf("AccScale.y:%f\n", scale[1]);
printf("AccScale.z:%f\n", scale[2]);
}
if(sensorType&SENSOR_MAG) {
CalBase=CAL_BASE_MAG;
nvtGetMagCalMatrix(matrix);
QualityFactor = nvtGetMagCalQFactor();
if(erase) {
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_INVALID));
return;
}
else
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_VALID));
for(i=0;i<MAG_CAL_DATA_SIZE;i++) {
DATA_FLASH_Write(CalBase++,float2dw(matrix[i]));
printf("MagInvW[%d]:%f\n", i, matrix[i]);
}
DATA_FLASH_Write(CalBase++,i162dw((int16_t)QualityFactor));
printf("Quality Factor:%d\n", QualityFactor);
}
}
void UpdateFlashCal(int8_t sensorType, bool erase)
{
uint8_t CalBase, i, QualityFactor;
float mean[3], scale[3],matrix[MAG_CAL_DATA_SIZE];
CAL_FLASH_STATE_T* FlashState;
FlashState = GetFlashState();
if(sensorType&SENSOR_GYRO) {
CalBase=CAL_BASE_GYRO;
nvtGetGyroOffset(mean);
nvtGetGyroScale(scale);
if(erase) {
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_INVALID));
FlashState->GYRO_FLASH = false;
return;
}
else
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_VALID));
FlashState->GYRO_FLASH = true;
DATA_FLASH_Write(CalBase++,float2dw(mean[0]));
DATA_FLASH_Write(CalBase++,float2dw(mean[1]));
DATA_FLASH_Write(CalBase++,float2dw(mean[2]));
DATA_FLASH_Write(CalBase++,float2dw(scale[0]));
DATA_FLASH_Write(CalBase++,float2dw(scale[1]));
DATA_FLASH_Write(CalBase++,float2dw(scale[2]));
if (report_format == REPORT_FORMAT_TEXT) {
printf("GyroMean.x:%f\n", mean[0]);
printf("GyroMean.y:%f\n", mean[1]);
printf("GyroMean.z:%f\n", mean[2]);
printf("GyroScale.x:%f\n", scale[0]);
printf("GyroScale.y:%f\n", scale[1]);
printf("GyroScale.z:%f\n", scale[2]);
}
}
if(sensorType&SENSOR_ACC) {
CalBase=CAL_BASE_ACC;
nvtGetAccOffset(mean);
nvtGetAccScale(scale);
if(erase) {
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_INVALID));
FlashState->ACC_FLASH = false;
return;
}
else
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_VALID));
FlashState->ACC_FLASH = true;
DATA_FLASH_Write(CalBase++,float2dw(mean[0]));
DATA_FLASH_Write(CalBase++,float2dw(mean[1]));
DATA_FLASH_Write(CalBase++,float2dw(mean[2]));
DATA_FLASH_Write(CalBase++,float2dw(scale[0]));
DATA_FLASH_Write(CalBase++,float2dw(scale[1]));
DATA_FLASH_Write(CalBase++,float2dw(scale[2]));
if (report_format == REPORT_FORMAT_TEXT) {
printf("AccMean.x:%f\n", mean[0]);
printf("AccMean.y:%f\n", mean[1]);
printf("AccMean.z:%f\n", mean[2]);
printf("AccScale.x:%f\n", scale[0]);
printf("AccScale.y:%f\n", scale[1]);
printf("AccScale.z:%f\n", scale[2]);
}
}
if(sensorType&SENSOR_MAG) {
CalBase=CAL_BASE_MAG;
nvtGetMagCalMatrix(matrix);
QualityFactor = nvtGetMagCalQFactor();
if(erase) {
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_INVALID));
FlashState->MAG_FLASH = false;
FlashState->MAG_QFACTOR = 0xff;
SensorInitMAG();
return;
}
else
DATA_FLASH_Write(CalBase++,i162dw((int16_t)FIELD_VALID));
FlashState->MAG_FLASH = true;
FlashState->MAG_QFACTOR = QualityFactor;
for(i=0; i<MAG_CAL_DATA_SIZE; i++) {
DATA_FLASH_Write(CalBase++,float2dw(matrix[i]));
if (report_format == REPORT_FORMAT_TEXT) {
printf("MagInvW[%d]:%f\n", i, matrix[i]);
}
}
DATA_FLASH_Write(CalBase++,i162dw((int16_t)QualityFactor));
if (report_format == REPORT_FORMAT_TEXT) {
printf("Quality Factor:%d\n", QualityFactor);
}
}
}
