philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

started calibration fixing

Browse Source
This commit is contained in:
Jonas Holmberg 2016-10-20 11:09:35 +02:00
parent 622a61f17f
commit e75e8a7089
3 changed files with 151 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
UAV-ControlSystem/src/config/cli.c
View File

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

97
UAV-ControlSystem/src/drivers/accel_gyro.c
View File

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

6
UAV-ControlSystem/src/tasks_main.c
View File

@ -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)