diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index ee9b57e..5eda143 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -65,7 +65,10 @@ extern pidProfile_t PidProfile[PID_COUNT]; extern float accRollFineTune; extern float accPitchFineTune; -extern accel_t accelProfile; /*Struct profile for input data from sensor*/ +extern accel_t accelProfile; + +extern float throttleRate; +extern int HoverForce;/*Struct profile for input data from sensor*/ extern float Yaw; extern float YawU; @@ -83,6 +86,8 @@ void pidInit(); **************************************************************************/ void pidRun(uint8_t ID); +void readAcc(void); + void pidEproom(void); #endif /* FLIGHT_PID_H_ */ diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index c1dae17..c29afba 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -123,8 +123,8 @@ typedef enum #if defined(BARO) || defined(SONAR) TASK_ALTITUDE, #endif -#if BEEPER - TASK_BEEPER +#ifdef BEEPER + TASK_BEEPER, #endif //Debug tasks, ONLY to be used when testing diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index c1a66b0..9cd0e7f 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -175,7 +175,7 @@ typedef enum { #if defined(BARO) || defined(SONAR) EEPROM_PERIOD_ALTITUDE, #endif -#if BEEPER +#ifdef BEEPER EEPROM_PERIOD_BEEPER, #endif @@ -209,6 +209,8 @@ typedef enum { typedef enum { EEPROM_PID_GYRO, EEPROM_PID_ACCELEROMETER, + EEPROM_PID_COMPASS, + EEPROM_PID_BAROMETER, /* Counts the amount of settings in profile */ EEPROM_PROFILE_COUNT diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h index 805964d..d1c6560 100644 --- a/UAV-ControlSystem/inc/drivers/accel_gyro.h +++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h @@ -114,6 +114,7 @@ typedef struct accel_t { int16_t accelXraw, accelYraw, accelZraw; /* Raw accelerometer data */ int16_t offsetX, offsetY, offsetZ; /* Accelerometer offset raw values */ uint16_t accel1G; /* Sensitivity factor */ + float rollAngle, pitchAngle; } accel_t; /*********************************************************************** @@ -164,6 +165,8 @@ int mpu6000_read_fifo(gyro_t* gyro, int16_t* data_out); ***********************************************************************/ bool mpu6000_who_am_i(); +void mpu6000_read_angle(accel_t* accel, gyro_t* gyro); + #endif /* DRIVERS_ACCEL_GYRO_H_ */ diff --git a/UAV-ControlSystem/inc/drivers/barometer.h b/UAV-ControlSystem/inc/drivers/barometer.h index 2c47476..a22b488 100644 --- a/UAV-ControlSystem/inc/drivers/barometer.h +++ b/UAV-ControlSystem/inc/drivers/barometer.h @@ -1,26 +1,113 @@ -/* - * barometer.h - * - * Created on: 18 okt. 2016 - * Author: holmis - */ +/************************************************************************** +* NAME: barometer.h * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Used to provide an estimated altitude, in regards to the * +* lift of height that would represent zero meters in height. * +* * +* INFORMATION: Using I2C to communicate with the barometer a pressure and * +* temperature value can be obtained. These values can then be* +* used to estimate an altitude. Note that this is not an * +* altitude value relative to the ground underneath it. It is * +* relative to the position where the system was started from.* +* The start position of the system will indicate the zero * +* height. It is that position and only that one which will * +* be the compared height. * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ #ifndef DRIVERS_BAROMETER_H_ #define DRIVERS_BAROMETER_H_ +typedef enum { + CALCSTATE_D2_CALCULATION = 0, //Tell the sensor that we want to read D2 + CALCSTATE_D2_READ, //Read D2 from the sensor + CALCSTATE_D1_CALCULATION, //Tell the sensor that we want to read D1 + CALCSTATE_D1_READ, //Read D1 from the sensor + CALCSTATE_CALCULATE_PTA //preassure, temp, altidute calc +}calculationState; +/*********************************************************************** + * BRIEF: Initializes the barometer. * + * INFORMATION: Initializes the barometer and it needs to be called * + * before anything else when using the barometer. * + ***********************************************************************/ bool barometer_init(); +/*********************************************************************** + * BRIEF: Resets the barometer. * + * INFORMATION: Resets the barometer needs to be called after the init.* + * It will send a reset message over the I2C to the * + * barometer telling it that is should perform a reset. * + * This needs to be done or it wont be possible to read * + * data from the barometer. * + ***********************************************************************/ bool barometer_reset(); +/*********************************************************************** + * BRIEF: Calculates the values of the preassure, temperature and* + * altitude. * + * INFORMATION: This function needs to be called five times for the * + * data to be updated. This is because of some limitations* + * and to ensure the schedulability of the system it needs* + * to be divided. Firstly there is an inherit delay inside* + * the barometer sensor. To get data from the barometer a * + * message needs to be sent that tells the barometer to * + * prepare the data. This takes, depending on the amount * + * of sampling that is done up to 10 ms for the highest * + * amount of sampling. This also needs to be done two * + * times before that data can be calculated. Also since * + * the implementation uses a software I2C at the moment * + * because of some problems with the DMA implementation * + * the speed is not very high. Therefore sending several * + * messages and reading at the same time may take to long * + * time and could cause the system to be unschedulable. * + * Because of this the function is divided into different * + * cases: * + * 1: Prepare data. * + * 2: Read data. * + * 3: Prepare data. * + * 4: Read data. * + * 5: Calculate temperature, pressure and altitude. * + ***********************************************************************/ void barometer_CaclulateValues(); -double barometer_GetCurrentPreassure(); +/*********************************************************************** + * BRIEF: Retrieves the previously calculated pressure. * + * INFORMATION: Returns the last calculated pressure value. No * + * calculation is performed here so calling this will give* + * the same value until a new calculation has been * + * performed. * + ***********************************************************************/ +double barometer_GetCurrentPressure(); +/*********************************************************************** + * BRIEF: Retrieves the previously calculated temperature. * + * INFORMATION: Returns the last calculated temperature value. No * + * calculation is performed here so calling this will give* + * the same value until a new calculation has been * + * performed. * + ***********************************************************************/ double barometer_GetCurrentTemperature(); -float barometer_GetCurrentAltitudeBasedOnSeaLevel(); - +/*********************************************************************** + * BRIEF: Retrieves the previously calculated altitude. * + * INFORMATION: Returns the last calculated altitude value. No * + * calculation is performed here so calling this will give* + * the same value until a new calculation has been * + * performed. * + ***********************************************************************/ +float barometer_GetCurrentAltitude(); +/*********************************************************************** + * BRIEF: Gets the altitude based on the last number of values. * + * INFORMATION: Averages the value on the last few reading to get a more* + * accurate reading. * + ***********************************************************************/ +float barometer_GetCurrentAveragedtAltitude(); #endif /* DRIVERS_BAROMETER_H_ */ diff --git a/UAV-ControlSystem/inc/drivers/beeper.h b/UAV-ControlSystem/inc/drivers/beeper.h new file mode 100644 index 0000000..1196155 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/beeper.h @@ -0,0 +1,19 @@ +/* + * beeper.h + * + * Created on: 14 nov. 2016 + * Author: holmis + */ + +#ifndef DRIVERS_BEEPER_H_ +#define DRIVERS_BEEPER_H_ + +#include "stm32f4xx_revo.h" + +void initBeeper(uint16_t led_pin, GPIO_TypeDef* led_port); + +void busyWaitBeep(uint16_t beepTimeMs); + +#endif /* DRIVERS_BEEPER_H_ */ + + diff --git a/UAV-ControlSystem/inc/drivers/i2c_soft.h b/UAV-ControlSystem/inc/drivers/i2c_soft.h index 7d4dcd3..4256979 100644 --- a/UAV-ControlSystem/inc/drivers/i2c_soft.h +++ b/UAV-ControlSystem/inc/drivers/i2c_soft.h @@ -1,15 +1,24 @@ -/* - * i2c_soft.h - * - * Created on: 27 okt. 2016 - * Author: holmis - */ +/************************************************************************** +* NAME: i2c_soft.h * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Used to communicate via I2C in a SW simulated manner. * +* * +* INFORMATION: A software implementation of the I2C. It toggles the pins * +* that are used on and of to generate I2C messages. * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ #ifndef DRIVERS_I2C_SOFT_H_ #define DRIVERS_I2C_SOFT_H_ #include "stm32f4xx.h" +/* Struct used to create a soft i2c handler */ typedef struct { GPIO_TypeDef * i2c_Port; @@ -17,12 +26,28 @@ typedef struct uint16_t i2c_sda_pin; }I2C_SOFT_handle_t; - +/*********************************************************************** + * BRIEF: Initializes the SW I2C. + * INFORMATION: Initializes the SW I2C, needs to be done before any + * thing else. + ***********************************************************************/ void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile); +/*********************************************************************** + * BRIEF: Writes a message. + * INFORMATION: Tries to write to an address. reg is the message that is + * written to the addr. data is the size of the data that + * is written. + ***********************************************************************/ bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data); +/*********************************************************************** + * BRIEF: Reads a message. + * INFORMATION: Tries to read a message from addr. reg is the message + * that says a read is desired. len is the length of the + * message that should be read and buf is the buffer that + * will store the read data. + ***********************************************************************/ bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf); - #endif /* DRIVERS_I2C_SOFT_H_ */ diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index 90f7cf2..6f6ad01 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -156,8 +156,9 @@ /* Beeper */ -//#define BEEPER - +#define BEEPER +#define BEEPER_PIN GPIO_PIN_12 +#define BEEPER_PORT GPIOB /* Define all the moter of the system, servos + extra */ diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h index b570055..33854b1 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 111 +#define EEPROM_SYS_VERSION 114 #define ADC_STATE #include "stm32f4xx.h" diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h index b8fa784..1f0bba8 100644 --- a/UAV-ControlSystem/inc/utilities.h +++ b/UAV-ControlSystem/inc/utilities.h @@ -83,6 +83,11 @@ uint32_t accumulate(uint32_t list[], int length); ***********************************************************************/ void Error_Handler(void); +/************************************************************************** +* BRIEF: Constrain float values within a defined limit * +* INFORMATION: Used in PID loop to limit values * +**************************************************************************/ +float constrainf(float amt, int low, int high); uint8_t reverse(uint8_t byte); diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 1d5d870..9585fb2 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -21,6 +21,8 @@ #include "drivers/motormix.h" #include "utilities.h" #include "drivers/arduino_com.h" +#include "drivers/barometer.h" +#include "drivers/system_clock.h" #define sq(x) ((x)*(x)) #define map(x, in_min, in_max, out_min, out_max) (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min @@ -28,17 +30,20 @@ #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ #define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/ -#define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/ +#define DTERM_SCALE 0.0529f /*D-term used as a scale value to the PID controller*/ +#define BAROMETER_SCALE 5 #define RADIO_RANGE 500 /*Radio range input*/ -#define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/ -#define ACCELEROMETER_RANGE 60 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ +#define BAROMETER_RANGE 10 /*Determines the range of the maximum height (limits the rc input)*/ +#define ACCELEROMETER_RANGE 30 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ #define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/ +#define DESIRED_HEIGHT 5 /*Height value in meters*/ + /*Struct that belongs to a certain PID controller*/ typedef struct pidProfileBuff_s { @@ -112,6 +117,8 @@ float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, co { return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv); } +float throttleRate = 1; +int HoverForce = 1475; /*Struct profile for input data from sensor*/ /************************************************************************** * BRIEF: Scales data from input range to output range * @@ -139,6 +146,8 @@ float constrainf(float amt, int low, int high) int i = 0; +uint8_t FlagVelocityLimit = 0; +float VelocityCompensation = 0; /************************************************************************** * BRIEF: Update current sensor values * @@ -146,6 +155,11 @@ int i = 0; **************************************************************************/ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) { + static float last_micros = 0; + static float oldHeightValue = 0; + float current_micros = 0; + float delta_t_baro = 0; + float current_height = 0; switch (ID_profile) @@ -161,66 +175,35 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_ACCELEROMETER: - mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ + sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune; + sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; -// -// if (calcGravity(accelProfile) > 1.15) -// { -// -// sensorValues[ROLL] = gyroProfile.gyroY*PidProfileBuff[ROLL].dT; -// sensorValues[PITCH] = gyroProfile.gyroX*PidProfileBuff[PITCH].dT; -// -// } -// else -// { + /*Checks the biggest angle */ + throttleRate = (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? 2 - cos(sensorValues[ROLL]*M_PI/180) : 2 - cos(sensorValues[PITCH]*M_PI/180); + + break; + case PID_ID_COMPASS: + + sensorValues[ROLL] = 0; + sensorValues[PITCH] = 0; + sensorValues[YAW] = 0; - float alpha = 0.5; - /*May need Low pass filter since the accelerometer may drift*/ + break; + case PID_ID_BAROMETER: - float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + current_micros = clock_get_us(); + current_micros = current_micros/1000000; + delta_t_baro = (current_micros - last_micros); + current_height = barometer_GetCurrentAveragedtAltitude(); - /*TODO add finetune for roll and pitch*/ - X_roll += accRollFineTune; - X_pitch += accPitchFineTune; + last_micros = current_micros; + sensorValues[0] = ((current_height - oldHeightValue)/delta_t_baro); + //sensorValues[0] = ((sensorValues[0] < - 2) || (sensorValues[0] > 2))? sensorValues[0]:0; - oldSensorValueRoll[i] = X_roll; - oldSensorValuePitch[i] = X_pitch; - - float RollValue = 0; - float PitchValue = 0; - - for (int ii = 0; ii < 12; ii++) - { - RollValue = RollValue + oldSensorValueRoll[ii]; - PitchValue = PitchValue + oldSensorValuePitch[ii]; - - } - - - i = (i < 11)? i + 1:0; - - sensorValues[ROLL] = RollValue/12; - sensorValues[PITCH] = PitchValue/12; - - sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0]; - sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1]; -// - oldSensorValue[0] = sensorValues[ROLL]; - oldSensorValue[1] = sensorValues[PITCH]; - - - -// sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); -// sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - - //float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - //sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT); - - //float sensorPitch = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - //sensorValues[PITCH] = pt1FilterApply4(&accelFilter[1], sensorPitch, 90, PidProfileBuff[PITCH].dT); + oldHeightValue = current_height; + sensorValues[0]*=BAROMETER_SCALE; break; case PID_ID_COMPASS: @@ -279,6 +262,9 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_BAROMETER: default: + current_micros = clock_get_us(); + current_micros = current_micros/1000000; + last_micros = current_micros; sensorValues[ROLL] = 0; sensorValues[PITCH] = 0; @@ -295,6 +281,10 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) **************************************************************************/ void getPointRate(float *desiredCommand, uint8_t ID_profile) { + float currentThrottle = 0; + float velocity = 0; + + //*Do something smart*// switch (ID_profile) { @@ -335,7 +325,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) break; case PID_ID_BAROMETER: - desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle); + currentThrottle = rc_input.Throttle - 1500; + velocity = (currentThrottle < - 20 || currentThrottle > 20 )? currentThrottle:0; + desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, velocity)*BAROMETER_SCALE; break; default: @@ -349,9 +341,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) * the controller **************************************************************************/ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, - float desiredValue, float errorAxis, uint8_t axis) + float desiredValue, float sensorValue, uint8_t axis) { - const float rateError = desiredValue - errorAxis; + const float rateError = desiredValue - sensorValue; /* -----calculate P component ---- */ @@ -372,12 +364,10 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, } } - /* -----calculate I component ---- */ float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. - // I coefficient (I) moved before integration to make limiting independent from PID settings ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); // Anti windup protection @@ -390,12 +380,6 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm); } -// if (motorLimitReached) -// { -// ITerm = pidProfileBuff->lastITerm[axis]; -// } - - pidProfileBuff->lastITerm[axis] = ITerm; @@ -446,16 +430,34 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, **************************************************************************/ void pidUAV(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff) { - float errorAxis[3] = { 0 }; /*Array of errors for each axis*/ - float pointRate[3] = { 0 }; /*Array of desired values for each axis*/ + float sensorValue[3] = { 0 }; /*Array of errors for each axis*/ + float desiredValue[3] = { 0 }; /*Array of desired values for each axis*/ - getCurrentValues(errorAxis, pidProfile->ID_profile); /*Get sensor values*/ - getPointRate(pointRate, pidProfile->ID_profile); /*Get reference values or desired values*/ + getCurrentValues(sensorValue, pidProfile->ID_profile); /*Get sensor values*/ + getPointRate(desiredValue, pidProfile->ID_profile); /*Get reference values or desired values*/ /* -------------PID controller------------- */ for (int axis = 0; axis < pidProfileBuff->DOF; axis++) { - pidUAVcore(pidProfile, pidProfileBuff, pointRate[axis], errorAxis[axis], axis); + pidUAVcore(pidProfile, pidProfileBuff, desiredValue[axis], sensorValue[axis], axis); + } +} + +uint8_t flagAccBuff = 0; + +void pidAccelerometer(void) +{ + static uint8_t counterAcc = 0; + + counterAcc = ((flagAccBuff == 0))? 0: counterAcc; + + pidUAV(&PidProfile[PID_ID_ACCELEROMETER], &PidProfileBuff[PID_ID_ACCELEROMETER]); + + if (counterAcc < 80) + { + PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll; + PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; + counterAcc +=1; } } @@ -488,10 +490,13 @@ void pidRun(uint8_t ID) { PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll; PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; + flagAccBuff = 0; + throttleRate = 1; } else { - pidUAV(&PidProfile[PID_ID_ACCELEROMETER], &PidProfileBuff[PID_ID_ACCELEROMETER]); + pidAccelerometer(); + flagAccBuff = 1; } break; @@ -509,13 +514,19 @@ void pidRun(uint8_t ID) break; case PID_ID_BAROMETER: - if (!PidProfile[PID_ID_BAROMETER].pidEnabled) + + if (!(PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id))) { PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle; } else { pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]); + PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -20, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); + + + //PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); + //PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); } break; @@ -524,6 +535,12 @@ void pidRun(uint8_t ID) } } +void readAcc(void) +{ + /*Reads data from accelerometer*/ + mpu6000_read_angle(&accelProfile,&gyroProfile); +} + /*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/ /************************************************************************** @@ -611,6 +628,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) PidProfile[ID].P[PITCH] = 135; PidProfile[ID].P[YAW] = 150; + PidProfile[ID].I[ROLL] = 50; + PidProfile[ID].I[PITCH] = 50; + PidProfile[ID].I[YAW] = 50; + PidProfile[ID].D[ROLL] = 75; PidProfile[ID].D[PITCH] = 95; PidProfile[ID].D[YAW] = 50; @@ -626,16 +647,16 @@ 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[ROLL] = 120; + PidProfile[ID].P[PITCH] = 250; PidProfile[ID].P[YAW] = 0; - PidProfile[ID].D[ROLL] = 40; - PidProfile[ID].D[PITCH] = 40; + PidProfile[ID].D[ROLL] = 0; + PidProfile[ID].D[PITCH] = 0; PidProfile[ID].D[YAW] = 0; - PidProfile[ID].PIDweight[ROLL] = 2; - PidProfile[ID].PIDweight[PITCH] = 2; + PidProfile[ID].PIDweight[ROLL] = 100; + PidProfile[ID].PIDweight[PITCH] = 100; PidProfile[ID].PIDweight[YAW] = 100; PidProfile[ID].pidEnabled = true; @@ -646,27 +667,21 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) 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; PidProfile[ID].pidEnabled = false; break; case PID_ID_BAROMETER: - PidProfile[ID].P[ROLL] = 10; - PidProfile[ID].P[PITCH] = 10; - PidProfile[ID].P[YAW] = 10; + PidProfile[ID].P[ROLL] = 1; PidProfile[ID].PIDweight[ROLL] = 100; - PidProfile[ID].PIDweight[PITCH] = 100; - PidProfile[ID].PIDweight[YAW] = 100; PidProfile[ID].pidEnabled = false; + PidProfile[ID].dterm_lpf = 90; + PidProfile[ID].pid_out_limit = 2000; break; default: @@ -697,9 +712,10 @@ void pidInit() void pidEproom(void) { + PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 200; - PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2; - PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; + PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 20; + PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 20; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms @@ -707,6 +723,5 @@ void pidEproom(void) PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000; PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000; - PidProfile[PID_ID_GYRO].I[YAW] = 40; } diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index 0086e91..7d5a5cb 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -361,7 +361,7 @@ void initSchedulerTasks(void) enableTask(TASK_ALTITUDE, true); #endif -#if BEEPER +#ifdef BEEPER enableTask(TASK_BEEPER, true); #endif } diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 12b5d09..e731d12 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -31,7 +31,7 @@ #define commandValueError 0xFFFFFFFFFFFFFFFF #define minSimilarCharacters 2 //the minimum amount of characters needed to to a search on a value -#define maxSimilarSearchValues 15 //max amount of values that will be found when doing a search for similar strings based on the written chars +#define maxSimilarSearchValues 18 //max amount of values that will be found when doing a search for similar strings based on the written chars #define CLI_baudRate 115200 //The baudrate used for the CLI usart #define msgArraySize 3 //The number of words that a max command can contain (ex: set looptime 1000) @@ -121,11 +121,11 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = { "| 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", - "| calibrate_motors - Calibrates all motors.", - "| calibrate_gyro - Calibrates the gyro.", - "| calibrate_accelerometer - Calibrates the accelerometer.", - "| calibrate_compass - Calibrates the compass.", - "| calibration_info_accelerometer - Provides info on the accelerometer calibration." + "| calibrate_motors - Calibrates all motors.\n\r", + "| calibrate_gyro - Calibrates the gyro.\n\r", + "| calibrate_accelerometer - Calibrates the accelerometer.\n\r", + "| calibrate_compass - Calibrates the compass.\n\r", + "| calibration_info_accelerometer - Provides info on the accelerometer calibration.\n\r" }; /* String array containing all the signature examples for each action */ @@ -199,7 +199,7 @@ typedef enum #if defined(BARO) || defined(SONAR) COMMAND_ID_PERIOD_ALTITUDE, #endif -#if BEEPER +#ifdef BEEPER COMMAND_ID_PERIOD_BEEPER, #endif @@ -285,9 +285,22 @@ typedef enum COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, COMMAND_ID_PID_ACCEL_OUT_LIMIT, + //Barometer + COMMAND_ID_PID_BARO_P_HEIGHT, + + COMMAND_ID_PID_BARO_I_HEIGHT, + + COMMAND_ID_PID_BARO_D_HEIGHT, + + COMMAND_ID_PID_BARO_DTERM_LPF, + COMMAND_ID_PID_BARO_PTERM_YAW_LPF, + COMMAND_ID_PID_BARO_YAW_P_LIMIT, + COMMAND_ID_PID_BARO_OUT_LIMIT, + /* Enable the different pid loops */ COMMAND_ID_PID_GYRO_ISENABLED, COMMAND_ID_PID_ACCEL_ISENABLED, + COMMAND_ID_PID_BARO_ISENABLED, /* Counter for the amount of commands */ COMMAND_ID_COUNT, @@ -406,7 +419,7 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { "task_period_altitude", COMMAND_ID_PERIOD_ALTITUDE, EEPROM_PERIOD_ALTITUDE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_32, .valueRange = {0, 1000000000} }, #endif -#if BEEPER +#ifdef BEEPER [COMMAND_ID_PERIOD_BEEPER] = { "task_period_beeper", COMMAND_ID_PERIOD_BEEPER, EEPROM_PERIOD_BEEPER, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_32, .valueRange = {0, 1000000000} @@ -664,6 +677,41 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { "pid_accel_out_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000} }, + //BARO P + [COMMAND_ID_PID_BARO_P_HEIGHT] = + { + "pid_baro_p_height", COMMAND_ID_PID_BARO_P_HEIGHT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 255} + }, + + //BARO I + [COMMAND_ID_PID_BARO_I_HEIGHT] = + { + "pid_baro_i_height", COMMAND_ID_PID_BARO_I_HEIGHT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 255} + }, + //BARO D + [COMMAND_ID_PID_BARO_D_HEIGHT] = + { + "pid_baro_d_height", COMMAND_ID_PID_BARO_D_HEIGHT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 255} + }, + + //BARO Filters and limits + [COMMAND_ID_PID_BARO_DTERM_LPF] = + { + "pid_baro_dterm_lpf", COMMAND_ID_PID_BARO_DTERM_LPF, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 65000} + }, + [COMMAND_ID_PID_BARO_PTERM_YAW_LPF] = + { + "pid_baro_pterm_yaw_lpf", COMMAND_ID_PID_BARO_PTERM_YAW_LPF, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 65000} + }, + [COMMAND_ID_PID_BARO_YAW_P_LIMIT] = + { + "pid_baro_yaw_p_limit", COMMAND_ID_PID_BARO_YAW_P_LIMIT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 65000} + }, + [COMMAND_ID_PID_BARO_OUT_LIMIT] = + { + "pid_baro_out_limit", COMMAND_ID_PID_BARO_OUT_LIMIT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000} + }, + /* Enable pid loops */ [COMMAND_ID_PID_GYRO_ISENABLED] = { @@ -673,6 +721,10 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { { "pid_accel_isenabled", COMMAND_ID_PID_ACCEL_ISENABLED, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1} }, + [COMMAND_ID_PID_BARO_ISENABLED] = + { + "pid_baro_isenabled", COMMAND_ID_PID_BARO_ISENABLED, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1} + }, }; diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index 2d3c2c3..2569742 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -195,7 +195,7 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = { .dataPtr = &(SystemTasks[TASK_ALTITUDE].desiredPeriod), }, #endif -#if BEEPER +#ifdef BEEPER [EEPROM_PERIOD_BEEPER] = { .size = sizeof(SystemTasks[TASK_BEEPER].desiredPeriod), @@ -297,6 +297,16 @@ EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = { .size = sizeof(pidProfile_t), .dataPtr = &(PidProfile[PID_ID_ACCELEROMETER]), }, + [EEPROM_PID_COMPASS] = + { + .size = sizeof(pidProfile_t), + .dataPtr = &(PidProfile[PID_ID_COMPASS]), + }, + [EEPROM_PID_BAROMETER] = + { + .size = sizeof(pidProfile_t), + .dataPtr = &(PidProfile[PID_ID_BAROMETER]), + }, }; /* Data pointers and sizes for footer content */ diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 759dcc5..00be629 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -7,6 +7,9 @@ #include #include "drivers/spi.h" +#include "utilities.h" +#include "math.h" +#include "drivers/system_clock.h" spi_profile mpu6000_spi_profile; uint8_t num_failed_receive = 0; @@ -243,6 +246,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel) HAL_Delay(60); + accel->pitchAngle = 0; + accel->rollAngle = 0; return true; } @@ -514,3 +519,79 @@ bool mpu6000_who_am_i() return false; } + + + + + +/* Set the Gyro Weight for Gyro/Acc complementary filter + Increasing this value would reduce and delay Acc influence on the output of the filter*/ +#define GYRO_ACC_DIV_FACTOR (2^16) // that means a CMP_FACTOR of 1024 (2^10) +#define ACCEL_LPF_FACTOR 16 + +#define GetMagnitude(x) (x*x) +#define Low_Magnitude (GetMagnitude(0.85)) +#define High_Magnitude (GetMagnitude(1.15)) + +/* Returns the angle. If magnitude of acc is out of range we calculate on integrated gyro data */ +void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) +{ + static uint32_t last_micros = 0; // Static stores micros measured from last iteration + uint32_t current_micros = clock_get_us(); + uint32_t delta_t = current_micros - last_micros; + last_micros = current_micros; + + float deltaGyroAngleFloat[3] = {0}; + static float lpf_Acc[3] = {0}; + static float smooth[3] = {0}; + float sign[3] = {0}; + float magnitude = 0; + + /* We read the accelerometer to get fresh data */ + mpu6000_read_accel(accel); + float accelConv[3] = {accel->accelXconv, accel->accelYconv, accel->accelZconv}; + + /* Filter part, go thorugh each axis */ + for (int i = 0; i < 3; i ++) + { + //Calculate a new smooth value based on a factor of the LPF value + smooth[i] = lpf_Acc[i] / ACCEL_LPF_FACTOR; + + //Save the sign(+/-) of the value + sign[i] = (accelConv[i]< 0) ? -1 : 1; + + //Calculate the new LPF value based on the raw sensor data - the smoothing value + lpf_Acc[i] += sign[i]*sqrtf(ABS_FLOAT(accelConv[i])) - smooth[i]; + + //recalculate the accelerometer data based on the smooth value, since we had to take the square root off the original value we square it to get in the original size + accelConv[i] = smooth[i] * smooth[i] * sign[i]; + + //calculate the magnitude of the gravitation for all axis + magnitude += ABS_FLOAT(accelConv[i]) * ABS_FLOAT(accelConv[i]); + + } + + //Calculate the approximate angle increase based on the gyros probable movement since the last invoke + deltaGyroAngleFloat[0] = (delta_t * (float)gyro->gyroX / 1000000.0); + deltaGyroAngleFloat[1] = (delta_t * (float)gyro->gyroY / 1000000.0) ; + deltaGyroAngleFloat[2] = (delta_t * (float)gyro->gyroZ / 1000000.0); + + //First integrate the gyro and add that to the angle calculation + accel->rollAngle += deltaGyroAngleFloat[1]; + accel->pitchAngle += deltaGyroAngleFloat[0]; + + //If the g forces of the accelerometer is within the given magnitude we will also add accelerometer data to the calculation + if (Low_Magnitude < magnitude && magnitude < High_Magnitude) + { + //Calculate the pure angle given by the accelerometer data + float a_RollAngle = -atan2(accelConv[0], sqrt(accelConv[1]*accelConv[1] + accelConv[2]*accelConv[2]))*180/M_PI; + float a_PitchAngle = atan2( accelConv[1], sqrt(accelConv[2]*accelConv[2] + accelConv[0]*accelConv[0]))*180/M_PI; + + //Check how much the accelerometer angle differs from the current calculated ange with the gyro. Add calculated factor to the real angle value + + accel->rollAngle += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR; + accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR; + + } + +} diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index b3fa766..ae62db4 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -1,9 +1,24 @@ -/* - * barometer.c - * - * Created on: 18 okt. 2016 - * Author: holmis - */ +/************************************************************************** +* NAME: barometer.c * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Used to provide an estimated altitude, in regards to the * +* lift of height that would represent zero meters in height. * +* * +* INFORMATION: Using I2C to communicate with the barometer a pressure and * +* temperature value can be obtained. These values can then be* +* used to estimate an altitude. Note that this is not an * +* altitude value relative to the ground underneath it. It is * +* relative to the position where the system was started from.* +* The start position of the system will indicate the zero * +* height. It is that position and only that one which will * +* be the compared height. * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ #include "drivers/barometer.h" #include "drivers/I2C.h" @@ -13,7 +28,7 @@ #include "drivers/i2c_soft.h" #include "drivers/failsafe_toggles.h" -#define Device_address_1 0x56 +#define Device_address_1 0x56 //Address of our device, not really important in this case #define ADDR_WRITE 0xEE // Module address write mode #define ADDR_READ 0xEF // Module address read mode @@ -31,38 +46,68 @@ #define CMD_ADC_4096 0x08 // ADC OSR=4096 #define CMD_PROM_RD 0xA0 // Prom read command -#define SEA_PRESS 1013.25 //default sea level pressure level in mb -#define FTMETERS 0.3048 //convert feet to meters +#define SEA_PRESS 1013.25 //default sea level pressure level in mb +#define FTMETERS 0.3048 //convert feet to meters -#define CALIBRATION_VAL_AMOUNT 30 +#define CALIBRATION_VAL_AMOUNT 40 +#define NUMB_AVERAGE_VALS 10 -I2C_HandleTypeDef baroI2C_handle; -DMA_HandleTypeDef baroI2C_Rx_DMA_handle; -DMA_HandleTypeDef baroI2C_Tx_DMA_handle; -I2C_SOFT_handle_t baroI2C_soft_handle; +I2C_HandleTypeDef baroI2C_handle; //Handle for the HW i2c (NOT USED ATM) +DMA_HandleTypeDef baroI2C_Rx_DMA_handle; //Dma handle receive (NOT USED ATM) +DMA_HandleTypeDef baroI2C_Tx_DMA_handle; //Dma handle for transmit (NOT USED ATM) +I2C_SOFT_handle_t baroI2C_soft_handle; //Handle for the SW i2c -uint8_t sampleAmount; +uint8_t sampleAmount; //The amount of samples to be used when by the barometer to calculate the needed variables +double baro_Preassure; // compensated pressure value (mB) +double baro_Temperature; // compensated temperature value (degC) +double baro_Altitude; // altitude +double baro_S; // sea level barometer (mB) -double baro_Preassure; // compensated pressure value (mB) -double baro_Temperature; // compensated temperature value (degC) -double baro_Altitude; // altitude (ft) -double baro_S; // sea level barometer (mB) +/* Calibration variables */ +float altitudeCalibrationValue = 0; //Value used as calibration value +float calibrationSamples[CALIBRATION_VAL_AMOUNT];//array of stored values to be used for calibration, only samples calibration values when machine is not armed +int calibrationSamplesCount = 0; //Counter for the amount of calibration samples +int calibrationSamplesIterator = 0; //Iterator for when going through all the samples -float altitudeCalibrationValue = 0; //Value used as calibration value -float calibrationSamples[CALIBRATION_VAL_AMOUNT]; //array of stored values to be used for calibration, only samples calibration values when machine is not armed -int calibrationSamplesCount = 0; -int calibrationSamplesIterator = 0; +/* average altitude variables */ +float averageAltitude[NUMB_AVERAGE_VALS]; +uint8_t averageAltitudeIterator = 0; +uint8_t averageAltitudeCount = 0; -//TODO: remove when not used for testing any more -uint32_t tempTestCounterStart = 0; - -uint8_t cobuf[3] = {0}; /* address: 0 = factory data and the setup * address: 1-6 = calibration coefficients * address: 7 = serial code and CRC */ -uint32_t coefficients_arr[8]; //coefficient storage +uint32_t coefficients_arr[8]; //coefficient storage +uint8_t cobuf[3] = {0}; //Array used when writing and reading data over the I2C +/*********************************************************************** + * BRIEF: Adds a new altitude value to the average buffer vals * + * INFORMATION: Will add the last calculated altitude value to the * + * buffer used to provide a average calc of altitude * + ***********************************************************************/ +void barometer_addAverageAltitudeSample() +{ + //fisrt check if the amount of samples is greater than the array + if (!(averageAltitudeCount >= NUMB_AVERAGE_VALS)) + averageAltitudeCount++; //if not increase the counter + + //Check if the iterator should restart from the beginning because of overflow + if (averageAltitudeIterator >= NUMB_AVERAGE_VALS) + averageAltitudeIterator = 0; //if it is set it to zero + + //Add the lates calculated altitude value to the samples + averageAltitude[averageAltitudeIterator] = baro_Altitude; + + //increase the iterator + averageAltitudeIterator ++; +} + +/*********************************************************************** + * BRIEF: Adds a new altitude value to the calibration samples. * + * INFORMATION: Will add the last calculated altitude value to the * + * buffer used to provide a calibration value. * + ***********************************************************************/ void barometer_addCalibrationSample() { //fisrt check if the amount of samples is greater than the array @@ -80,6 +125,16 @@ void barometer_addCalibrationSample() calibrationSamplesIterator ++; } +/*********************************************************************** + * BRIEF: Calibrates the barometers start position. * + * INFORMATION: An array of values are sampled as long as the system * + * is not armed. Upon arming the system the values in * + * the buffer will be averaged and this will give the * + * calibration value. In other words it will give the * + * height that represents zero. This value will be * + * subtracted from every altitude calculation that is * + * performed. * + ***********************************************************************/ bool barometer_Calibrate() { //Check if any calibration values exist @@ -108,6 +163,11 @@ bool barometer_Calibrate() return true; } +/*********************************************************************** + * BRIEF: Initializes the barometer. * + * INFORMATION: Initializes the barometer and it needs to be called * + * before anything else when using the barometer. * + ***********************************************************************/ bool barometer_init() { //Set the sample rate of the data that will be calculated on the barometer peripheral @@ -118,12 +178,20 @@ bool barometer_init() i2c_soft_Init(I2C1, &baroI2C_soft_handle); #endif #ifdef BARO_USE_I2C_HARD - + //Todo: needs implementation #endif return true; } +/*********************************************************************** + * BRIEF: Resets the barometer. * + * INFORMATION: Resets the barometer needs to be called after the init.* + * It will send a reset message over the I2C to the * + * barometer telling it that is should perform a reset. * + * This needs to be done or it wont be possible to read * + * data from the barometer. * + ***********************************************************************/ bool barometer_reset() { /* Send a reset command to the baromter @@ -142,6 +210,8 @@ bool barometer_reset() #endif #ifdef BARO_USE_I2C_HARD + //Todo: needs implementation + uint8_t cobuf2[3] = {0}; /* Change to hardware polling mode */ cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount); @@ -193,21 +263,21 @@ bool barometer_reset() /* Set the inital calibration value */ barometer_Calibrate(); + //force bakc the iscalibrated status to false flags_Clear_ID(systemFlags_barometerIsCalibrated_id); - tempTestCounterStart = clock_get_ms(); return true; } -typedef enum { - CALCSTATE_D2_CALCULATION = 0, //Tell the sensor that we want to read D2 - CALCSTATE_D2_READ, //Read D2 from the sensor - CALCSTATE_D1_CALCULATION, //Tell the sensor that we want to read D1 - CALCSTATE_D1_READ, //Read D1 from the sensor - CALCSTATE_CALCULATE_PTA //preassure, temp, altidute calc -}calculationState; - +/*********************************************************************** + * BRIEF: Calculates the values of temp, pres, and altitude. * + * INFORMATION: It takes in two values D1 and D2 which are the values * + * that have been read from the barometer. This values are* + * then used to perform the calculations together with * + * the coefficients that have been read in the reset * + * function. * + ***********************************************************************/ void barometer_CalculatePTA(uint32_t D1, uint32_t D2) { /* calculate dT, difference between actual and reference temp: (D2 - C5 * 2^8) */ @@ -243,10 +313,39 @@ void barometer_CalculatePTA(uint32_t D1, uint32_t D2) /* Calculate the altitude */ float feet = ((float)1 - (pow(((float)baro_Preassure / (float)SEA_PRESS), (float)0.190284))) * (float)145366.45; - baro_Altitude = (flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) ? (feet * FTMETERS) - altitudeCalibrationValue : (feet * FTMETERS); + + + /* Add altitude values to altitude buffer containing the last few readings */ + barometer_addAverageAltitudeSample(); } +/*********************************************************************** + * BRIEF: Calculates the values of the preassure, temperature and* + * altitude. * + * INFORMATION: This function needs to be called five times for the * + * data to be updated. This is because of some limitations* + * and to ensure the schedulability of the system it needs* + * to be divided. Firstly there is an inherit delay inside* + * the barometer sensor. To get data from the barometer a * + * message needs to be sent that tells the barometer to * + * prepare the data. This takes, depending on the amount * + * of sampling that is done up to 10 ms for the highest * + * amount of sampling. This also needs to be done two * + * times before that data can be calculated. Also since * + * the implementation uses a software I2C at the moment * + * because of some problems with the DMA implementation * + * the speed is not very high. Therefore sending several * + * messages and reading at the same time may take to long * + * time and could cause the system to be unschedulable. * + * Because of this the function is divided into different * + * cases: * + * 1: Prepare data. * + * 2: Read data. * + * 3: Prepare data. * + * 4: Read data. * + * 5: Calculate temperature, pressure and altitude. * + ***********************************************************************/ void barometer_CaclulateValues() { /*the calculation is in need of different states. This is because the @@ -254,7 +353,8 @@ void barometer_CaclulateValues() * use a delay wait we need to do parts of the calculation every time * the function is called. The "delay" is handled by the period of * the task that handles the calculation. It cant have a period faster - * that 10 ms, or the wait will not be enough in some cases according + * that 10 ms or 5ms or 2.5 ms and so on, depending on the CMD_ADC_ assigned + * to the variable "sampleAmount" The wait will not be enough in some cases according * to the datasheet of the sensor http://www.amsys.info/sheets/amsys.en.ms5611_01ba03.pdf*/ static uint8_t currentCalculationState = CALCSTATE_D2_CALCULATION; static uint32_t D1 = 0; @@ -263,7 +363,6 @@ void barometer_CaclulateValues() uint32_t startTime; uint32_t endTime; - //If the machine is armed and not calibrated we perform a calibraton if (!flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) { @@ -273,8 +372,6 @@ void barometer_CaclulateValues() } } - - switch (currentCalculationState) { case CALCSTATE_D2_CALCULATION: @@ -365,19 +462,88 @@ void barometer_CaclulateValues() } -double barometer_GetCurrentPreassure() +/*********************************************************************** + * BRIEF: Retrieves the previously calculated pressure. * + * INFORMATION: Returns the last calculated pressure value. No * + * calculation is performed here so calling this will give* + * the same value until a new calculation has been * + * performed. * + ***********************************************************************/ +double barometer_GetCurrentPressure() { return baro_Preassure; } +/*********************************************************************** + * BRIEF: Retrieves the previously calculated temperature. * + * INFORMATION: Returns the last calculated temperature value. No * + * calculation is performed here so calling this will give* + * the same value until a new calculation has been * + * performed. * + ***********************************************************************/ double barometer_GetCurrentTemperature() { return baro_Temperature; } -float barometer_GetCurrentAltitudeBasedOnSeaLevel() +/*********************************************************************** + * BRIEF: Retrieves the previously calculated altitude. * + * INFORMATION: Returns the last calculated altitude value. No * + * calculation is performed here so calling this will give* + * the same value until a new calculation has been * + * performed. * + ***********************************************************************/ +float barometer_GetCurrentAltitude() { return baro_Altitude; } +/*********************************************************************** + * BRIEF: Gets the altitude based on the last number of values. * + * INFORMATION: Averages the value on the last few reading to get a more* + * accurate reading. * + ***********************************************************************/ +float barometer_GetCurrentAveragedtAltitude() +{ +// float toReturn = 0; +// /* Go through all the values in the buffer */ +// for (int i = 0; i < averageAltitudeCount; i++) +// { +// toReturn += averageAltitude[i]; +// } +// +// /* Return the average of the stored values */ +// toReturn = toReturn/averageAltitudeCount; +// return toReturn; +// + + static float lpf_Acc = 0; + static float smooth = 0; + float toReturn = 0; + + + /* Filter part, go thorugh each axis */ + + //Calculate a new smooth value based on a factor of the LPF value + smooth = lpf_Acc / 16; + + //Calculate the new LPF value based on the raw sensor data - the smoothing value + lpf_Acc += baro_Altitude - smooth; + + //recalculate the accelerometer data based on the smooth value, since we had to take the square root off the original value we square it to get in the original size + // toReturn = smooth * smooth; + + return smooth; + + + + +// static float prevVal = 0; +// float toRet = (prevVal*6 + baro_Altitude) / 8; +// prevVal = baro_Altitude; +// return toRet; + + +} + diff --git a/UAV-ControlSystem/src/drivers/beeper.c b/UAV-ControlSystem/src/drivers/beeper.c new file mode 100644 index 0000000..f6a156e --- /dev/null +++ b/UAV-ControlSystem/src/drivers/beeper.c @@ -0,0 +1,34 @@ +/* + * beeper.c + * + * Created on: 14 nov. 2016 + * Author: holmis + */ + +#include "drivers/beeper.h" + + +uint16_t beeperPin; +GPIO_TypeDef* beeperPort; + +void initBeeper(uint16_t beeper_pin, GPIO_TypeDef* beeper_port) +{ + beeperPin = beeper_pin; + beeperPort = beeper_port; + + GPIO_InitTypeDef gpinit; + gpinit.Pin = beeper_pin; + gpinit.Mode = GPIO_MODE_OUTPUT_PP; + gpinit.Pull = GPIO_PULLUP; + gpinit.Speed = GPIO_SPEED_HIGH; + HAL_GPIO_Init(beeper_port, &gpinit); + +} + +void busyWaitBeep(uint16_t beepTimeMs) +{ + /* If you use this in the scheduled part of the code, you might face a problem with a little bit of a crash ok? */ + HAL_GPIO_WritePin(beeperPort, beeperPin, SET); + HAL_Delay(beepTimeMs); + HAL_GPIO_WritePin(beeperPort, beeperPin, RESET); +} diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c index 2821950..9576e59 100644 --- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c +++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c @@ -40,9 +40,9 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = { .flagId = systemFlags_flightmode_acceleromter_id, }, [FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER] = { - .minRange = 0, - .maxRange = 0, - .channelNumber = 0, + .minRange = 1900, + .maxRange = 2100, + .channelNumber = 6, .flagId = systemFlags_flightmode_barometer_id, }, [FLAG_CONFIGURATION_FLIGHTMODE_COMPASS] = { diff --git a/UAV-ControlSystem/src/drivers/i2c_soft.c b/UAV-ControlSystem/src/drivers/i2c_soft.c index 34bf083..48c6592 100644 --- a/UAV-ControlSystem/src/drivers/i2c_soft.c +++ b/UAV-ControlSystem/src/drivers/i2c_soft.c @@ -11,21 +11,37 @@ #define WRITE_INDICATOR 0 #define READ_INDICATOR 1 +/*********************************************************************** + * BRIEF: set given pin to high + * INFORMATION: + ***********************************************************************/ static void IOHi(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) { HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET); } +/*********************************************************************** + * BRIEF: Set given pin to low + * INFORMATION: + ***********************************************************************/ static void IOLo(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) { HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET); } +/*********************************************************************** + * BRIEF: Read given ii pin + * INFORMATION: + ***********************************************************************/ static bool IORead(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) { return !! (GPIOx->IDR & GPIO_Pin); } +/*********************************************************************** + * BRIEF: Delay for a few micros + * INFORMATION: + ***********************************************************************/ static void i2c_soft_delay(void) { volatile int i = 1; @@ -34,6 +50,11 @@ static void i2c_soft_delay(void) } } +/*********************************************************************** + * BRIEF: Initializes the SW I2C. + * INFORMATION: Initializes the SW I2C, needs to be done before any + * thing else. + ***********************************************************************/ void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile) { uint16_t sda_pin, scl_pin; @@ -67,6 +88,10 @@ void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile) out_profile->i2c_sda_pin = sda_pin; } +/*********************************************************************** + * BRIEF: Starts the i2c + * INFORMATION: + ***********************************************************************/ static bool i2c_soft_Start(I2C_SOFT_handle_t *handle) { IOHi(handle->i2c_Port, handle->i2c_sda_pin); @@ -85,6 +110,10 @@ static bool i2c_soft_Start(I2C_SOFT_handle_t *handle) return true; } +/*********************************************************************** + * BRIEF: Stops the i2c + * INFORMATION: + ***********************************************************************/ static void i2c_soft_Stop(I2C_SOFT_handle_t *handle) { IOLo(handle->i2c_Port, handle->i2c_scl_pin); @@ -97,6 +126,10 @@ static void i2c_soft_Stop(I2C_SOFT_handle_t *handle) asm ("nop"); // i2c_soft_delay(); } +/*********************************************************************** + * BRIEF: Sends ack + * INFORMATION: + ***********************************************************************/ static void i2c_soft_Ack(I2C_SOFT_handle_t *handle) { IOLo(handle->i2c_Port, handle->i2c_scl_pin); @@ -109,6 +142,10 @@ static void i2c_soft_Ack(I2C_SOFT_handle_t *handle) asm ("nop"); // i2c_soft_delay(); } +/*********************************************************************** + * BRIEF: Sends no ack + * INFORMATION: + ***********************************************************************/ static void i2c_soft_NoAck(I2C_SOFT_handle_t *handle) { IOLo(handle->i2c_Port, handle->i2c_scl_pin); @@ -121,6 +158,10 @@ static void i2c_soft_NoAck(I2C_SOFT_handle_t *handle) asm ("nop"); // i2c_soft_delay(); } +/*********************************************************************** + * BRIEF: Wait for an acknowledge. + * INFORMATION: Waits for an acknowledge when a message has been sent. + ***********************************************************************/ static bool i2c_soft_WaitAck(I2C_SOFT_handle_t *handle) { IOLo(handle->i2c_Port, handle->i2c_scl_pin); @@ -137,6 +178,10 @@ static bool i2c_soft_WaitAck(I2C_SOFT_handle_t *handle) return true; } +/*********************************************************************** + * BRIEF: Sends a byte. + * INFORMATION: Sends the value byte over the i2c. + ***********************************************************************/ static void i2c_soft_SendByte(I2C_SOFT_handle_t *handle, uint8_t byte) { uint8_t i = 8; @@ -157,6 +202,10 @@ static void i2c_soft_SendByte(I2C_SOFT_handle_t *handle, uint8_t byte) IOLo(handle->i2c_Port, handle->i2c_scl_pin); } +/*********************************************************************** + * BRIEF: Receives a byte. + * INFORMATION: Receives a byte and stores is in the byte value. + ***********************************************************************/ static uint8_t i2c_soft_ReceiveByte(I2C_SOFT_handle_t *handle) { uint8_t i = 8; @@ -177,6 +226,13 @@ static uint8_t i2c_soft_ReceiveByte(I2C_SOFT_handle_t *handle) return byte; } +/*********************************************************************** + * BRIEF: Reads a message. + * INFORMATION: Tries to read a message from addr. reg is the message + * that says a read is desired. len is the length of the + * message that should be read and buf is the buffer that + * will store the read data. + ***********************************************************************/ bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) { //just send the addres 0x77 @@ -212,6 +268,12 @@ bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t return true; } +/*********************************************************************** + * BRIEF: Writes a message. + * INFORMATION: Tries to write to an address. reg is the message that is + * written to the addr. data is the size of the data that + * is written. + ***********************************************************************/ bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data) { //just send the addres 0x77 diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 0def06d..25ba171 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -103,8 +103,9 @@ void mix() int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array int16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor - int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]; // Import throttle value from remote + int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]*throttleRate; + if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += HoverForce; /* Mixer Full Scale enabled */ if (flags_IsSet_ID(systemFlags_mixerfullscale_id)) @@ -182,7 +183,11 @@ void mix() // Now we add desired throttle for (int i = 0; i < MOTOR_COUNT; i++) // Constrain in within the regulation of the mix - OBS. Constrain can be removed. Just to make sure + + // TODO: This line i>>>>>>>>>s backup as we discovered that motors could stop at times in airmode on M-UAV. But we have not seen this before motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax); + //motor_output[i] = constrain(RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax), throttleMin, throttleMax); + } else // Mixer full scale NOT active { @@ -287,6 +292,13 @@ void mix() else motor_output[i] = mixerConfig.minCommand; + + /* TODO: This is temp fix to be able to disable all motors but one */ +// int enabled_motorA = 0; +// int enabled_motorB = 5; +// if (i != enabled_motorA && i != enabled_motorB) +// motor_output[i] = mixerConfig.minCommand; + /* Update actuators */ pwmAdjustSpeedOfMotor( i + 1 /* Motors start from Motor 1 */,motor_output[i]); } diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c index 53bb569..1a44d5c 100644 --- a/UAV-ControlSystem/src/drivers/motors.c +++ b/UAV-ControlSystem/src/drivers/motors.c @@ -14,6 +14,7 @@ #include "drivers/motors.h" #include "drivers/failsafe_toggles.h" #include "config/eeprom.h" +#include "drivers/motormix.h" const int MotorPWMPeriode = 2000; //Micro seconds const int MotorPWMInitPulse = 1000; @@ -162,7 +163,11 @@ void pwmEnableMotor(uint8_t motor, motorOutput motorOutput) **************************************************************************/ void pwmEnableAllMotors(motorOutput motorOutput) { - for (uint8_t i = 1; i < 11; i++ ) pwmEnableMotor(i, motorOutput); + for (uint8_t i = 1; i < 11; i++ ) + { + pwmEnableMotor(i, motorOutput); + pwmAdjustSpeedOfMotor(i,mixerConfig.minCommand); + } } /************************************************************************** diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index a2960e5..25ad347 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -30,6 +30,7 @@ #include "Flight/pid.h" #include "drivers/barometer.h" #include "drivers/arduino_com.h" +#include "drivers/beeper.h" /************************************************************************** * BRIEF: Should contain all the initializations of the system, needs to @@ -78,8 +79,8 @@ void init_system() #endif #ifdef BARO - //barometer_init(); - //barometer_reset(); + barometer_init(); + barometer_reset(); #endif #ifdef COMPASS @@ -98,8 +99,8 @@ void init_system() #endif -#if BEEPER - +#ifdef BEEPER + initBeeper(BEEPER_PIN, BEEPER_PORT); #endif @@ -122,6 +123,9 @@ int main(void) //Light the yellow led ledOnInverted(Led1, Led1_GPIO_PORT); + //beep that it has been initialized + busyWaitBeep(1000); + //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 12ef234..4115c84 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -43,21 +43,14 @@ #include "Flight/pid.h" #include "drivers/barometer.h" #include "drivers/arduino_com.h" +#include "drivers/beeper.h" void systemTaskGyroPid(void) { - //Read gyro and update PID and finally update the motors. The most important task in the system - - //Update Gyro - - //Convert? //PID Gyro pidRun(PID_ID_GYRO); - //MIX GO - - //call the motor mix mix(); } @@ -65,10 +58,8 @@ void systemTaskGyroPid(void) void systemTaskAccelerometer(void) { + readAcc(); pidRun(PID_ID_ACCELEROMETER); - //update the accelerometer data -// uint8_t c = 97; -// usart_transmit(&cliUsart, &c, 1, 1000000000); } void systemTaskAttitude(void) @@ -113,7 +104,6 @@ void systemTaskRx(void) continuousMissedFrames = (frame.flag_FrameLost) ? continuousMissedFrames + 1 : 0; (continuousMissedFrames > 10) ? flags_Set_ID(systemFlags_Failsafe_toManyMissedFrames_id) : flags_Clear_ID(systemFlags_Failsafe_toManyMissedFrames_id); - } bool systemTaskRxCheck(uint32_t currentDeltaTime) @@ -164,6 +154,7 @@ bool systemTaskRxCliCheck(uint32_t currentDeltaTime) return false; } +//TODO: change the name of this task. Could be something like void systemTaskSerial(void) { static bool readyToCalibrate = true; @@ -181,23 +172,30 @@ void systemTaskSerial(void) // mpu6000_read_acc_offset(&accelProfile); // } // } + + //If we are ready to accept a new calibration value. You can only perform one calibration until the sticks have been centered once until the next calibration if(readyToCalibrate) { + //If any calibration is performed set readyToCalibrate to false so it cant just increase indefinitely when holding the sticks in a certain position if (flags_IsSet_ID(systemFlags_stickLeft_id)) { accRollFineTune -= calibrationAmount; + readyToCalibrate = false; } else if (flags_IsSet_ID(systemFlags_stickRight_id)) { accRollFineTune += calibrationAmount; + readyToCalibrate = false; } else if (flags_IsSet_ID(systemFlags_stickUp_id)) { accPitchFineTune -= calibrationAmount; + readyToCalibrate = false; } else if (flags_IsSet_ID(systemFlags_stickDown_id)) { accPitchFineTune += calibrationAmount; + readyToCalibrate = false; } } @@ -233,7 +231,7 @@ void systemTaskArduino(void) void systemTaskBaro(void) { - //barometer_CaclulateValues(); + barometer_CaclulateValues(); } void systemTaskCompass(void) @@ -253,11 +251,16 @@ void systemTaskSonar(void) void systemTaskAltitude(void) { +// uint8_t c[50]; +// sprintf(c, "Roll: %-6f, Pitch %-6f\r", accelProfile.rollAngle, accelProfile.pitchAngle); +// usart_transmit(&cliUsart, &c, 50, 1000000000); + //Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar //double temperature = barometer_GetCurrentTemperature(); //double pressure = barometer_GetCurrentPreassure(); - //float altitute = barometer_GetCurrentAltitudeBasedOnSeaLevel(); + //float altitute = barometer_GetCurrentAltitude(); + //float altitute = barometer_GetCurrentAveragedtAltitude(); //pid run, should probably be moved to systemTaskAltitude pidRun(PID_ID_BAROMETER); diff --git a/UAV-ControlSystem/src/utilities.c b/UAV-ControlSystem/src/utilities.c index 7c2192c..4e824d0 100644 --- a/UAV-ControlSystem/src/utilities.c +++ b/UAV-ControlSystem/src/utilities.c @@ -228,3 +228,17 @@ int16_t constrain(int16_t value, int16_t min, int16_t max) else return value; } + +/************************************************************************** +* BRIEF: Constrain float values within a defined limit * +* INFORMATION: Used in PID loop to limit values * +**************************************************************************/ +float constrainf(float amt, int low, int high) +{ + if (amt < (float)low) + return (float)low; + else if (amt > (float)high) + return (float)high; + else + return amt; +}