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); } } }