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

Added calibration for accelerometer

Browse Source
This commit is contained in:
Jonas Holmberg 2016-10-28 14:34:49 +02:00
parent e1b7a5c2ff
commit 0f37f6600d
14 changed files with 266 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
UAV-ControlSystem/inc/Flight/pid.h
View File

@ -18,6 +18,7 @@
#include<stdio.h>
#include<stdint.h>
#include "Flight/filter.h"
#include "drivers/accel_gyro.h"
#define XYZ_AXIS_COUNT 3 /*The maximum number of DOF that belongings to the PID*/
@ -60,6 +61,12 @@ typedef struct pidProfile_s {
/*Array of all pid profiles of the system*/
extern pidProfile_t PidProfile[PID_COUNT];
/* */
extern float accRollFineTune;
extern float accPitchFineTune;
extern accel_t accelProfile; /*Struct profile for input data from sensor*/
/*Is set in motor mix and used in pidUAVcore and mix */
bool motorLimitReached;

9
UAV-ControlSystem/inc/config/eeprom.h
View File

@ -191,6 +191,15 @@ typedef enum {
EEPROM_FLAG_MIXERLOWSCALE,
EEPROM_FLAG_FLIGHTMODE_3,
/* accel calibration values */
EEPROM_FLAG_ACCELTUNE_OFFSET_X,
EEPROM_FLAG_ACCELTUNE_OFFSET_Y,
EEPROM_FLAG_ACCELTUNE_OFFSET_Z,
/* accel calibration fine tune values */
EEPROM_FLAG_ACCELTUNE_FINE_ROLL,
EEPROM_FLAG_ACCELTUNE_FINE_PITCH,
/* Counts the amount of system settings */
EEPROM_SYS_COUNT
} EEPROM_SYS_ID_t;

2
UAV-ControlSystem/inc/drivers/accel_gyro.h
View File

@ -27,7 +27,7 @@
#ifndef DRIVERS_ACCEL_GYRO_H_
#define DRIVERS_ACCEL_GYRO_H_
#include <stdbool.h>
//#include <stdbool.h>
#include "stm32f4xx.h"
#include "stm32f4xx_revo.h"

38
UAV-ControlSystem/inc/drivers/failsafe_toggles.h
View File

@ -47,6 +47,15 @@
#define systemFlags_flightMode_3_id 7 + boolean_vals_offset
#define systemFlags_barometerIsCalibrated_id 8 + boolean_vals_offset
#define systemFlags_AcceleromterIsCalibrated_id 9 + boolean_vals_offset
/* Stick booleans */
#define systemFlags_throttleMax_id 10 + boolean_vals_offset
#define systemFlags_stickLeft_id 11 + boolean_vals_offset
#define systemFlags_stickRight_id 12 + boolean_vals_offset
#define systemFlags_stickUp_id 13 + boolean_vals_offset
#define systemFlags_stickDown_id 14 + boolean_vals_offset
#define systemFlags_stickCenterH_id 15 + boolean_vals_offset
#define systemFlags_stickCenterV_id 16 + boolean_vals_offset
#define systemFlags_throttleLeft_id 17 + boolean_vals_offset
/*Mask values for each of the flag values*/
@ -66,6 +75,15 @@
#define systemFlags_flightMode_3_mask getFlagMaskValue(systemFlags_flightMode_3_id)
#define systemFlags_barometerIsCalibrated_mask getFlagMaskValue(systemFlags_barometerIsCalibrated_id)
#define systemFlags_AcceleromterIsCalibrated_mask getFlagMaskValue(systemFlags_AcceleromterIsCalibrated_id)
/* Stick booleans */
#define systemFlags_throttleMax_mask getFlagMaskValue(systemFlags_throttleMax_id)
#define systemFlags_stickLeft_mask getFlagMaskValue(systemFlags_stickLeft_id)
#define systemFlags_stickRight_mask getFlagMaskValue(systemFlags_stickRight_id
#define systemFlags_stickUp_mask getFlagMaskValue(systemFlags_stickUp_id)
#define systemFlags_stickDown_mask getFlagMaskValue(systemFlags_stickDown_id)
#define systemFlags_stickCenterH_mask getFlagMaskValue(systemFlags_stickCenterH_id)
#define systemFlags_stickCenterV_mask getFlagMaskValue(systemFlags_stickCenterV_id)
#define systemFlags_throttleLeft_mask getFlagMaskValue(systemFlags_throttleLeft_id)
@ -92,6 +110,16 @@ typedef union bitSetRegion
booleanValue_t flightMode_3 : 1;
booleanValue_t barometerIsCalibrated : 1;
booleanValue_t AcceleromterIsCalibrated : 1;
/* Stick booleans */
booleanValue_t throttleMax : 1;
booleanValue_t stickLeft : 1;
booleanValue_t stickRight : 1;
booleanValue_t stickUp : 1;
booleanValue_t stickDown : 1;
booleanValue_t stickCenterH : 1;
booleanValue_t stickCenterV : 1;
booleanValue_t throttleLeft : 1;
}bitField;
uint64_t intRepresentation;
}boolFlags_t;
@ -115,6 +143,16 @@ typedef enum
FLAG_CONFIGURATION_MIXERFULLSCALE,
FLAG_CONFIGURATION_MIXERLOWSCALE,
FLAG_CONFIGURATION_FLIGHTMODE_3,
/* Stick booleans */
FLAG_CONFIGURATION_THROTTLEMAX,
FLAG_CONFIGURATION_STICKLEFT,
FLAG_CONFIGURATION_STICKRIGHT,
FLAG_CONFIGURATION_STICKUP,
FLAG_CONFIGURATION_STICKDOWN,
FLAG_CONFIGURATION_STICKCENTERH,
FLAG_CONFIGURATION_STICKCENTERV,
FLAG_CONFIGURATION_THROTTLELEFT,
//Counter
FLAG_CONFIGURATION_COUNT

2
UAV-ControlSystem/inc/system_variables.h
View File

@ -14,7 +14,7 @@
#ifndef SYSTEM_VARIABLES_H_
#define SYSTEM_VARIABLES_H_
#define EEPROM_SYS_VERSION 102
#define EEPROM_SYS_VERSION 107
#define ADC_STATE
#include "stm32f4xx.h"

41
UAV-ControlSystem/src/Flight/pid.c
View File

@ -14,7 +14,6 @@
* **************************************************************************/
#include "Flight/pid.h"
#include "drivers/accel_gyro.h"
#include "drivers/sbus.h"
#include "scheduler/scheduler.h"
#include <math.h>
@ -53,11 +52,14 @@ typedef struct pidProfileBuff_s {
pidProfile_t PidProfile[PID_COUNT] = {0}; /*Array of all pid profiles of the system*/
pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0};
accel_t accelProfile; /*Struct profile for input data from sensor*/
gyro_t gyroProfile; /*Struct profile for input data from sensor*/
accel_t accelProfile = {0}; /*Struct profile for input data from sensor*/
gyro_t gyroProfile = {0}; /*Struct profile for input data from sensor*/
pt1Filter_t accelFilter[2] = {0};
float accRollFineTune = 0;
float accPitchFineTune = 0;
/**************************************************************************
* BRIEF: Calculates angle from accelerometer *
* INFORMATION: *
@ -151,6 +153,11 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
/*TODO add finetune for roll and pitch*/
X_roll += accRollFineTune;
X_pitch += accPitchFineTune;
oldSensorValueRoll[i] = X_roll;
oldSensorValuePitch[i] = X_pitch;
@ -499,14 +506,20 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
PidProfile[ID].PID_Out[YAW] = 0;
PidProfile[ID].P[ROLL] = 10;
PidProfile[ID].P[PITCH] = 10;
PidProfile[ID].P[YAW] = 10;
switch (ID)
{
case PID_ID_GYRO:
PidProfile[ID].P[ROLL] = 150;
PidProfile[ID].P[PITCH] = 135;
PidProfile[ID].P[YAW] = 150;
PidProfile[ID].D[ROLL] = 75;
PidProfile[ID].D[PITCH] = 95;
PidProfile[ID].D[YAW] = 50;
PidProfile[ID].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100;
@ -518,6 +531,14 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
break;
case PID_ID_ACCELEROMETER:
PidProfile[ID].P[ROLL] = 90;
PidProfile[ID].P[PITCH] = 90;
PidProfile[ID].P[YAW] = 0;
PidProfile[ID].D[ROLL] = 40;
PidProfile[ID].D[PITCH] = 40;
PidProfile[ID].D[YAW] = 0;
PidProfile[ID].PIDweight[ROLL] = 2;
PidProfile[ID].PIDweight[PITCH] = 2;
PidProfile[ID].PIDweight[YAW] = 100;
@ -529,6 +550,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
break;
case PID_ID_COMPASS:
PidProfile[ID].P[ROLL] = 10;
PidProfile[ID].P[PITCH] = 10;
PidProfile[ID].P[YAW] = 10;
PidProfile[ID].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100;
@ -538,6 +563,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
break;
case PID_ID_BAROMETER:
PidProfile[ID].P[ROLL] = 10;
PidProfile[ID].P[PITCH] = 10;
PidProfile[ID].P[YAW] = 10;
PidProfile[ID].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100;

2
UAV-ControlSystem/src/Scheduler/tasks.c
View File

@ -88,7 +88,7 @@ task_t SystemTasks[TASK_COUNT] =
{
.taskName = "SERIAL",
.taskFunction = systemTaskSerial,
.desiredPeriod = GetUpdateRateHz(100), //100 hz update rate (10 ms)
.desiredPeriod = GetUpdateRateHz(2), //100 hz update rate (10 ms)
.staticPriority = PRIORITY_LOW,
},

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

@ -24,6 +24,8 @@
#include "utilities.h"
#include "Scheduler/scheduler.h"
#include "drivers/motors.h"
#include "drivers/accel_gyro.h"
#include "Flight/pid.h"
#define cliActivateCharacter 35
#define commandValueError 0xFFFFFFFFFFFFFFFF
@ -76,6 +78,7 @@ typedef enum
ACTION_GYROCALIBRATE,
ACTION_ACCELEROMTETERCALIBRATE,
ACTION_COMPASSCALIBRATE,
ACTION_CALIBRATIONINFOACCEL,
//The number of actions
ACTION_COUNT, //Count of the number of actions
@ -100,7 +103,8 @@ const typeString commandAction_Strings[ACTION_COUNT] = {
"calibrate_motors",
"calibrate_gyro",
"calibrate_accelerometer",
"calibrate_compass"
"calibrate_compass",
"calibration_info_accelerometer"
};
/* String values descrbing information of a certain action */
@ -120,7 +124,8 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = {
"| calibrate_motors - Calibrates all motors.",
"| calibrate_gyro - Calibrates the gyro.",
"| calibrate_accelerometer - Calibrates the accelerometer.",
"| calibrate_compass - Calibrates the compass."
"| calibrate_compass - Calibrates the compass.",
"| calibration_info_accelerometer - Provides info on the accelerometer calibration."
};
/* String array containing all the signature examples for each action */
@ -140,7 +145,8 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = {
" calibrate_motors",
" calibrate_gyro",
" calibrate_accelerometer",
" calibrate_compass"
" calibrate_compass",
" calibration_info_accelerometer"
};
/* Size values for the values a command will require */
@ -1477,7 +1483,6 @@ void cliRun()
if (ReceiveBinaryDecision(121,110))
{
TransmitBack("Starting calibration! \n\n\r");
TransmitBack("NOT IMPLEMENTED! \n\n\r");
TransmitBack("Calibration complete! \n\n\r");
}
else
@ -1500,6 +1505,15 @@ void cliRun()
}
break;
case commandMask(commandSize_1, ACTION_CALIBRATIONINFOACCEL):
{
char tempBuffer[100];
TransmitBack("- Accelerometer calibration information: \n\n\r");
TransmitBack(tempBuffer);
sprintf(tempBuffer, "- BaseTune: \n\r-OffsetX: %d \n\r-OffsetY: %d \n\r-OffsetZ: %d \n\n\r", accelProfile.offsetX, accelProfile.offsetY, accelProfile.offsetZ);
break;
}
default:
if (actionId != ACTION_NOACTION)
TransmitCommandInstruction(actionId);

29
UAV-ControlSystem/src/config/eeprom.c
View File

@ -248,9 +248,34 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
.dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_3]),
},
/* accel calibration values */
[EEPROM_FLAG_ACCELTUNE_OFFSET_X] =
{
.size = sizeof(int16_t),
.dataPtr = &(accelProfile.offsetX),
},
[EEPROM_FLAG_ACCELTUNE_OFFSET_Y] =
{
.size = sizeof(int16_t),
.dataPtr = &(accelProfile.offsetY),
},
[EEPROM_FLAG_ACCELTUNE_OFFSET_Z] =
{
.size = sizeof(int16_t),
.dataPtr = &(accelProfile.offsetZ),
},
/* accel fine tune */
[EEPROM_FLAG_ACCELTUNE_FINE_ROLL] =
{
.size = sizeof(float),
.dataPtr = &(accRollFineTune),
},
[EEPROM_FLAG_ACCELTUNE_FINE_PITCH] =
{
.size = sizeof(float),
.dataPtr = &(accPitchFineTune),
},
};
/* Data pointers and sizes for profile content */

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

@ -239,7 +239,7 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
return false;
mpu6000_read_gyro_offset(gyro);
mpu6000_read_acc_offset(accel);
//mpu6000_read_acc_offset(accel);
HAL_Delay(60);

74
UAV-ControlSystem/src/drivers/failsafe_toggles.c
View File

@ -29,14 +29,14 @@ boolFlags_t systemFlags = {{0}};
flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
[FLAG_CONFIGURATION_ARM] = {
.minRange = 1500,
.maxRange = 2000,
.maxRange = 2100,
.channelNumber = 8,
.flagId = systemFlags_armed_id,
},
[FLAG_CONFIGURATION_FLIGHTMODE_ACCELEROMETER] = {
.minRange = 1600,
.maxRange = 2000,
.channelNumber = 5,
.minRange = 1400,
.maxRange = 2100,
.channelNumber = 6,
.flagId = systemFlags_flightmode_acceleromter_id,
},
[FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER] = {
@ -58,15 +58,15 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
.flagId = systemFlags_flightmode_gps_id,
},
[FLAG_CONFIGURATION_MIXERFULLSCALE] = {
.minRange = 0,
.maxRange = 0,
.channelNumber = 0,
.minRange = 1900,
.maxRange = 2100,
.channelNumber = 5,
.flagId = systemFlags_mixerfullscale_id,
},
[FLAG_CONFIGURATION_MIXERLOWSCALE] = {
.minRange = 0,
.maxRange = 0,
.channelNumber = 0,
.minRange = 900,
.maxRange = 1100,
.channelNumber = 5,
.flagId = systemFlags_mixerlowscale_id,
},
[FLAG_CONFIGURATION_FLIGHTMODE_3] = {
@ -74,7 +74,59 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
.maxRange = 0,
.channelNumber = 0,
.flagId = systemFlags_flightMode_3_id,
},
/*Stick controls*/
[FLAG_CONFIGURATION_THROTTLEMAX] = {
.minRange = 1950,
.maxRange = 2000,
.channelNumber = 3,
.flagId = systemFlags_throttleMax_id,
},
[FLAG_CONFIGURATION_STICKLEFT] = { //negative
.minRange = 1000,
.maxRange = 1100,
.channelNumber = 1,
.flagId = systemFlags_stickLeft_id,
},
[FLAG_CONFIGURATION_STICKRIGHT] = { //positive
.minRange = 1900,
.maxRange = 2000,
.channelNumber = 1,
.flagId = systemFlags_stickRight_id,
},
[FLAG_CONFIGURATION_STICKUP] = { //negative
.minRange = 1000,
.maxRange = 1100,
.channelNumber = 2,
.flagId = systemFlags_stickUp_id,
},
[FLAG_CONFIGURATION_STICKDOWN] = { //positive
.minRange = 1900,
.maxRange = 2000,
.channelNumber = 2,
.flagId = systemFlags_stickDown_id,
},
[FLAG_CONFIGURATION_STICKCENTERH] = {
.minRange = 1400,
.maxRange = 1600,
.channelNumber = 1,
.flagId = systemFlags_stickCenterH_id,
},
[FLAG_CONFIGURATION_STICKCENTERV] = {
.minRange = 1400,
.maxRange = 1600,
.channelNumber = 2,
.flagId = systemFlags_stickCenterV_id,
},
[FLAG_CONFIGURATION_THROTTLELEFT] = {
.minRange = 2000,
.maxRange = 1900,
.channelNumber = 4,
.flagId = systemFlags_throttleLeft_id,
}
};
/***********************************************************************
@ -123,7 +175,7 @@ void flags_ProcessRcChannel_Improved(uint8_t minChannel, uint8_t maxChannel)
{
int currentChannelNumb = flagConfigArr[i].channelNumber;
//Check if the current Flag channel is within the set bounds
if(currentChannelNumb >= 5 && currentChannelNumb <= maxChannel)
if(currentChannelNumb >= minChannel && currentChannelNumb <= maxChannel)
{
//Get the value for the channel that the current flag is linked to
int value = getChannelValue(sbusChannelData, currentChannelNumb);

4
UAV-ControlSystem/src/drivers/motormix.c
View File

@ -45,14 +45,14 @@ uint16_t motor_output[MOTOR_COUNT];
/* Default values for the mixerConfig */
// TODO: Implement in EEPROM
mixerConfig_s mixerConfig = {
.minThrottle = 1050,
.minThrottle = 1040,
.maxThrottle = MAX_PULSE - 100,
.minCommand = 990,
.maxCommand = MAX_PULSE,
.minCheck = 1010,
.pid_at_min_throttle = true,
.motorstop = false,
.yaw_reverse_direction = false
.yaw_reverse_direction = true
};
/* Used in "mixerUAV" to create the dynamic model of the UAV */

11
UAV-ControlSystem/src/main.c
View File

@ -61,9 +61,7 @@ void init_system()
//init sbus, using USART1
sbus_init();
//init motors to run with oneshot 125, small delay
HAL_Delay(1000);
pwmEnableAllMotors(Oneshot125);
#ifdef USE_LEDS
@ -99,6 +97,10 @@ void init_system()
#endif
//init motors to run with oneshot 125, small delay
HAL_Delay(1000);
pwmEnableAllMotors(Oneshot125);
}
/**************************************************************************
@ -114,6 +116,9 @@ int main(void)
//Init the system
init_system();
//Light the yellow led
ledOnInverted(Led1, Led1_GPIO_PORT);
//Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler
initScheduler();

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

@ -92,7 +92,10 @@ void systemTaskRx(void)
// }
/*Updated flag processRcChannel function, not yet tested. Should work as the entire loop above*/
flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
//flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
/* Includes the stick channel in the toggles checks */
flags_ProcessRcChannel_Improved(1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
//temporary send data from the RC directly form the RC
// RawRcCommand.Roll = frame.chan1;
@ -162,24 +165,63 @@ bool systemTaskRxCliCheck(uint32_t currentDeltaTime)
void systemTaskSerial(void)
{
// uint8_t c = 118;
// usart_transmit(&cliUsart, &c, 1, 1000000000);
if (flags_IsSet_ID(systemFlags_armed_id))
ledOnInverted(Led0_PIN, Led0_GPIO_PORT);
else
ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
static bool readyToCalibrate = true;
const float calibrationAmount = 0.5;
//Only run if the system is not armed
if (!flags_IsSet_ID(systemFlags_armed_id))
{
if (flags_IsSet_ID(systemFlags_throttleMax_id))
{
// if(flags_IsSet_ID(systemFlags_throttleLeft_id))
// {
// if(flags_IsSet_ID(systemFlags_stickDown_id))
// {
// mpu6000_read_acc_offset(&accelProfile);
// }
// }
if(readyToCalibrate)
{
if (flags_IsSet_ID(systemFlags_stickLeft_id))
{
accRollFineTune -= calibrationAmount;
}
else if (flags_IsSet_ID(systemFlags_stickRight_id))
{
accRollFineTune += calibrationAmount;
}
else if (flags_IsSet_ID(systemFlags_stickUp_id))
{
accPitchFineTune -= calibrationAmount;
}
else if (flags_IsSet_ID(systemFlags_stickDown_id))
{
accPitchFineTune += calibrationAmount;
}
}
else
{
//if the stick is centered set ready to calibrate to true
if(flags_IsSet_MASK(systemFlags_stickCenterH_mask | systemFlags_stickCenterV_mask))
{
readyToCalibrate = true;
}
}
}
}
}
void systemTaskBattery(void)
{
//Keep track of the battery level of the system
// uint8_t c = 98;
// usart_transmit(&cliUsart, &c, 1, 1000000000);
if (flags_IsSet_MASK((systemFlags_flightmode_acceleromter_mask | systemFlags_armed_mask)))
ledOnInverted(Led1, Led1_GPIO_PORT);
// uint8_t c = 118;
// usart_transmit(&cliUsart, &c, 1, 1000000000);
if (flags_IsSet_ID(systemFlags_armed_id))
ledOnInverted(Led0_PIN, Led0_GPIO_PORT);
else
ledOffInverted(Led1, Led1_GPIO_PORT);
ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
}
void systemTaskBaro(void)