From e75e8a70893f5e9c25a3391b8fb7cb3b1709bc92 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 20 Oct 2016 11:09:35 +0200 Subject: [PATCH] started calibration fixing --- UAV-ControlSystem/src/config/cli.c | 85 +++++++++++++++---- UAV-ControlSystem/src/drivers/accel_gyro.c | 97 +++++++++++++++++----- UAV-ControlSystem/src/tasks_main.c | 6 +- 3 files changed, 151 insertions(+), 37 deletions(-) diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 7f96636..f53a4e8 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -73,6 +73,9 @@ typedef enum ACTION_RESET, //Resets the entire eeprom ACTION_STATS, //gives statistics on the system ACTION_MOTORCALIBRATE, + ACTION_GYROCALIBRATE, + ACTION_ACCELEROMTETERCALIBRATE, + ACTION_COMPASSCALIBRATE, //The number of actions ACTION_COUNT, //Count of the number of actions @@ -94,7 +97,10 @@ const typeString commandAction_Strings[ACTION_COUNT] = { "reboot", "reset", "stats", - "motorcalibrate" + "calibrate_motors", + "calibrate_gyro", + "calibrate_accelerometer", + "calibrate_compass" }; /* String values descrbing information of a certain action */ @@ -110,8 +116,11 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = { "| exit - Exits the CLI mode.\n\r", "| reboot - Exit CLI and reboots the system.\n\r", "| reset - Restore all the values to its default values.\n\r", - "| stats - Gives some current stats of the system and tasks.\n\r" - "| motorcalibrate - Calibrates all motors." + "| stats - Gives some current stats of the system and tasks.\n\r", + "| calibrate_motors - Calibrates all motors.", + "| calibrate_gyro - Calibrates the gyro.", + "| calibrate_accelerometer - Calibrates the accelerometer.", + "| calibrate_compass - Calibrates the compass." }; /* String array containing all the signature examples for each action */ @@ -128,7 +137,10 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = { " reboot", " reset", " stats", - " motorcalibrate" + " calibrate_motors", + " calibrate_gyro", + " calibrate_accelerometer", + " calibrate_compass" }; /* Size values for the values a command will require */ @@ -1398,17 +1410,17 @@ void cliRun() break; case commandMask(commandSize_1, ACTION_RESET): //resets all the values in the eeprom to the default values - TransmitBack("- All values will be deleted and system rebooted. Continue? (y/n)... \n\n\r"); + TransmitBack("- All values will be deleted and system rebooted. Continue? (y/n)... \n\n\r"); - //read until a y or n is found - if (ReceiveBinaryDecision(121, 110)) - { - defaultEEPROM(); - } - else - { - TransmitBack("- Values unchanged...\n\n\r"); - } + //read until a y or n is found + if (ReceiveBinaryDecision(121, 110)) + { + defaultEEPROM(); + } + else + { + TransmitBack("- Values unchanged...\n\n\r"); + } break; case commandMask(commandSize_1, ACTION_STATS): TransmitSystemStats(); @@ -1422,13 +1434,56 @@ void cliRun() { TransmitBack("Starting calibration, BE CAREFUL!!! \n\n\r"); calibrateMotors(); + TransmitBack("Wait until the beeping have ceased... \n\n\r"); } else { TransmitBack("Exiting the calibration, BE CAREFUL!!! \n\n\r"); } - break; + break; + case commandMask(commandSize_1, ACTION_GYROCALIBRATE): + TransmitBack("Do you really want to calibrate the gyro? (y/n)\n\n\r"); + if (ReceiveBinaryDecision(121,110)) + { + TransmitBack("Starting calibration! \n\n\r"); + TransmitBack("NOT IMPLEMENTED! \n\n\r"); + TransmitBack("Calibration complete! \n\n\r"); + } + else + { + TransmitBack("Exiting the calibration! \n\n\r"); + } + + break; + case commandMask(commandSize_1, ACTION_ACCELEROMTETERCALIBRATE): + TransmitBack("Do you really want to calibrate the accelerometer? (y/n)\n\n\r"); + if (ReceiveBinaryDecision(121,110)) + { + TransmitBack("Starting calibration! \n\n\r"); + TransmitBack("NOT IMPLEMENTED! \n\n\r"); + TransmitBack("Calibration complete! \n\n\r"); + } + else + { + TransmitBack("Exiting the calibration! \n\n\r"); + } + + break; + case commandMask(commandSize_1, ACTION_COMPASSCALIBRATE): + TransmitBack("Do you really want to calibrate the compass? (y/n)\n\n\r"); + if (ReceiveBinaryDecision(121,110)) + { + TransmitBack("Starting calibration! \n\n\r"); + TransmitBack("NOT IMPLEMENTED! \n\n\r"); + TransmitBack("Calibration complete! \n\n\r"); + } + else + { + TransmitBack("Exiting the calibration! \n\n\r"); + } + + break; default: if (actionId != ACTION_NOACTION) TransmitCommandInstruction(actionId); diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 95ae8d0..a40e668 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -44,7 +44,7 @@ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32 /*********************************************************************** * BRIEF: mpu6000_read_offset reads and returns the offset of the * - * gyroscope and accelerometer * + * gyroscope * * INFORMATION: * * Is automatically called when mpu6000_init is called * * The flight controller needs to be stationary when this function is * @@ -52,46 +52,104 @@ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32 * When the UAV is finished this data could be saved so that the * * offset doesn't need to be read every time * ***********************************************************************/ -HAL_StatusTypeDef mpu6000_read_offset(gyro_t* gyro, accel_t* accel) +HAL_StatusTypeDef mpu6000_read_gyro_offset(gyro_t* gyro) { uint8_t dataG[6]; /* Temporary data variable used to receive gyroscope data */ - uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */ + int16_t dataGCheck[6] = {0}; /* checkval */ + uint8_t maxDrift = 5; + bool calibrate_go = true; + bool calibrate_ok = false; + int countCalibrate = 0; - if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10)) - return HAL_ERROR; + //small delay so that we know the gyro should give some values + HAL_Delay(100); + for (int i = 0; i < 100; i++) + { + //assume that we should calibrate at the start of each loop + calibrate_go = true; + + //get new values + if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10)) + return HAL_ERROR; + + //cheack the new values and see if they are within acceptable range compared to the previous values, so that it is realativelly still + for(int j = 0; j < 6; j ++) + { + int16_t currentDrift = (int16_t)abs(dataGCheck[j] - dataG[j]); + if (!(currentDrift < maxDrift)) + calibrate_go = false; + + dataGCheck[j] = dataG[j]; + } + + //if true, we have good values exit loop + if (calibrate_go) + { + countCalibrate++; + } + if (countCalibrate > 4) + { + calibrate_ok = true; + break; + } + //wait for a little bit so the checks are not to fast + HAL_Delay(200); + } - if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10)) - return HAL_ERROR; #ifdef YAW_ROT_0 gyro->offsetX = -(((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetY = -(((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); - accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1]; - accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3]; - accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_90) gyro->offsetX = -(((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetY = (((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); - - accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3]; - accel->offsetY = ((int16_t)dataA[0] << 8) | dataA[1]; - accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_180) gyro->offsetX = (((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetY = (((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); - - accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1]; - accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3]; - accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_270) gyro->offsetX = (((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetY = -(((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); +#endif + return HAL_OK; +} + +/*********************************************************************** + * BRIEF: mpu6000_read_offset reads and returns the offset of the * + * accelerometer * + * INFORMATION: * + * Is automatically called when mpu6000_init is called * + * The flight controller needs to be stationary when this function is * + * called * + * When the UAV is finished this data could be saved so that the * + * offset doesn't need to be read every time * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_acc_offset(accel_t* accel) +{ + uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */ + + if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10)) + return HAL_ERROR; + + +#ifdef YAW_ROT_0 + accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_90) + accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetY = ((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_180) + accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_270) accel->offsetX = ((int16_t)dataA[2] << 8) | dataA[3]; accel->offsetY = -((int16_t)dataA[0] << 8) | dataA[1]; accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); @@ -180,7 +238,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel) if(!mpu6000_transmit(reg, 2)) return false; - mpu6000_read_offset(gyro, accel); + mpu6000_read_gyro_offset(gyro); + mpu6000_read_acc_offset(accel); return true; } diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 8984962..ad801a9 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -183,7 +183,7 @@ void systemTaskBattery(void) void systemTaskBaro(void) { - barometer_CaclulateValues(); + //barometer_CaclulateValues(); @@ -261,10 +261,10 @@ void systemTaskSonar(void) void systemTaskAltitude(void) { //Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar - double temperature = barometer_GetCurrentTemperature(); + /*double temperature = barometer_GetCurrentTemperature(); char buffer[50]; sprintf(buffer, "Temperature: %8.2f \n\r", temperature); - usart_transmit(&cliUsart, buffer, 50, 1000000000); + usart_transmit(&cliUsart, buffer, 50, 1000000000);*/ } void systemTaskBeeper(void)