diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index a19bd81..39728c7 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -18,6 +18,7 @@ #include #include #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; diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 282761d..1774a35 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -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; diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h index fd42421..805964d 100644 --- a/UAV-ControlSystem/inc/drivers/accel_gyro.h +++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h @@ -27,7 +27,7 @@ #ifndef DRIVERS_ACCEL_GYRO_H_ #define DRIVERS_ACCEL_GYRO_H_ -#include +//#include #include "stm32f4xx.h" #include "stm32f4xx_revo.h" diff --git a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h index 0a0d3bc..8e1044f 100644 --- a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h +++ b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h @@ -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 diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h index 607b8e1..0217fd9 100644 --- a/UAV-ControlSystem/inc/system_variables.h +++ b/UAV-ControlSystem/inc/system_variables.h @@ -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" diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index b614907..c595d16 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -14,7 +14,6 @@ * **************************************************************************/ #include "Flight/pid.h" -#include "drivers/accel_gyro.h" #include "drivers/sbus.h" #include "scheduler/scheduler.h" #include @@ -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; diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c index aa17518..f06d65d 100644 --- a/UAV-ControlSystem/src/Scheduler/tasks.c +++ b/UAV-ControlSystem/src/Scheduler/tasks.c @@ -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, }, diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index e6fda2c..80481cf 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -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); diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index c58f327..7852cc1 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -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 */ diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index c77a4f7..759dcc5 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -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); diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c index 3101c34..58b00ce 100644 --- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c +++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c @@ -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); diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 3da3fec..1261837 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -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 */ diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 3b7cdcd..c4af970 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -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(); diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 00b15d7..4392405 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -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)