void AccCalibration()
{
const char direction = GetChar();
const char calibration_done = 'd';
char side = atoi(&direction);
signed char status;
if (direction == 'x')
side = 0;
else
side = atoi(&direction);
if ((direction == '0')||(direction == 'x'))
nvtCalACCInit();
do {
DelayMsec(1);
SensorsRead(SENSOR_ACC,1);
status = nvtCalACCBufferFill(side);
}while(status==STATUS_BUFFER_NOT_FILLED);
if(status==STATUS_BUFFER_FILLED) {
if (direction == 'x')
UpdateFlashCal(SENSOR_ACC, false);
Serial_write((char*)&direction, 1);
}
else {
Serial_write((char*)&calibration_done, 1);
UpdateFlashCal(SENSOR_ACC, false);
}
}
void GyroCalibration()
{
const char axis_done = GetChar();
const char axis = axis_done - 0x78;
const char calibration_done = 'd';
signed char status;
nvtCalGyroInit(axis);
do {
SensorsRead(SENSOR_GYRO,1);
#ifndef OPTION_RC
DelayMsec(5);
#else
DelayMsec(16);
#endif
//printf("T:%d\n",getTickCount());
status=nvtGyroScaleCalibrate(axis);
led_arm_state(LED_STATE_TOGGLE);
UpdateLED();
} while(status==STATUS_GYRO_CAL_RUNNING);
if(status==STATUS_GYRO_AXIS_CAL_DONE)
Serial_write((char*)&axis_done, 1);
else {
Serial_write((char*)&calibration_done, 1);
UpdateFlashCal(SENSOR_GYRO, false);
}
}
void SensorCalibration()
{
char InitState = GetSensorInitState();
char calibration_sensor = GetChar();
#ifdef OPTION_RC
TIMER_Enable(false);
RC_Enable(false);
#endif
if((calibration_sensor=='a')&&(InitState&SENSOR_ACC)) { // Do 'a'cc calibration
AccCalibration();
}
else if((calibration_sensor=='g')&&(InitState&SENSOR_GYRO)) {// Do 'g'yro calibration
GyroCalibration();
}
else if((calibration_sensor=='m')&&(InitState&SENSOR_MAG)) { // Do 'g'yro calibration
MagCalibration();
if(CalQFactor<MAG_CAL_SUCESS_TH)
UpdateFlashCal(SENSOR_MAG, false);
SensorInitMAG();
}
else // Fail doing calibration
CalibrationFail();
#ifdef OPTION_RC
TIMER_Enable(true);
RC_Enable(true);
#endif
}
void GyroCalibration()
{
const char axis_done = GetChar();
const char axis = axis_done - 0x78;
const char calibration_done = 'd';
signed char status;
nvtCalGyroInit(axis);
do
{
SensorsRead(SENSOR_GYRO,1);
DelayMsec(16);
status=nvtGyroScaleCalibrate(axis);
}
while(status==STATUS_GYRO_CAL_RUNNING);
if(status==STATUS_GYRO_AXIS_CAL_DONE)
Serial_write((char*)&axis_done, 1);
else
{
Serial_write((char*)&calibration_done, 1);
UpdateFlashCal(SENSOR_GYRO, false);
}
}
void FlashControl()
{
char Action = GetChar();
if(Action=='e') {
char Part = GetChar();
if(Part=='a') {
UpdateFlashCal(SENSOR_ACC, true);
if (report_format == REPORT_FORMAT_TEXT) {
printf("ACC CalData Erased.\n");
}
}
else if(Part=='g') {
UpdateFlashCal(SENSOR_GYRO, true);
if (report_format == REPORT_FORMAT_TEXT) {
printf("GYRO CalData Erased.\n");
}
}
else if(Part=='m') {
UpdateFlashCal(SENSOR_MAG, true);
if (report_format == REPORT_FORMAT_TEXT) {
printf("MAG CalData Erased.\n");
}
}
else if(Part=='p') {
UpdateFlashPID(true);
if (report_format == REPORT_FORMAT_TEXT) {
printf("PID Erased.\n");
}
}
else if(Part=='b') {
UpdateBoardVersion(true);
if (report_format == REPORT_FORMAT_TEXT) {
printf("Board Code Erased.\n");
}
}
else if(Part=='x') {
UpdateFlashCal(SENSOR_ACC, true);
UpdateFlashCal(SENSOR_GYRO, true);
UpdateFlashCal(SENSOR_MAG, true);
UpdateFlashPID(true);
if (report_format == REPORT_FORMAT_TEXT) {
printf("Flash Data Erased.\n");
}
}
}
}
void SensorCalibration()
{
char InitState = GetSensorInitState();
char calibration_sensor = GetChar();
TIMER_Enable(false);
if((calibration_sensor=='a')&&(InitState&SENSOR_ACC)) // Do 'a'cc calibration
{
AccCalibration();
}
else if((calibration_sensor=='g')&&(InitState&SENSOR_GYRO)) // Do 'g'yro calibration
{
GyroCalibration();
}
else if((calibration_sensor=='m')&&(InitState&SENSOR_MAG)) // Do 'g'yro calibration
{
MagCalibration();
UpdateFlashCal(SENSOR_MAG, false);
}
else // Fail doing calibration
CalibrationFail();
TIMER_Enable(true);
}