diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index b8de8ad..6f6ad01 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -157,7 +157,7 @@ /* Beeper */ #define BEEPER -#define BEEPER_PIN 12 +#define BEEPER_PIN GPIO_PIN_12 #define BEEPER_PORT GPIOB diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h index eac6e2f..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 110 +#define EEPROM_SYS_VERSION 114 #define ADC_STATE #include "stm32f4xx.h" diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index b9dd38c..31027e9 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -21,12 +21,13 @@ #include "drivers/motormix.h" #include "utilities.h" #include "drivers/barometer.h" +#include "drivers/system_clock.h" #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 BAROMETER_SCALE 11 +#define BAROMETER_SCALE 5 #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 10 /*Determines the range of the maximum height (limits the rc input)*/ @@ -90,6 +91,10 @@ 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) { @@ -123,20 +128,24 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_BAROMETER: - float current_micros = clock_get_us()/1000000; - float delta_t = (current_micros - last_micros); - float current_height = barometer_GetCurrentAveragedtAltitude(); + current_micros = clock_get_us(); + current_micros = current_micros/1000000; + delta_t_baro = (current_micros - last_micros); + current_height = barometer_GetCurrentAveragedtAltitude(); last_micros = current_micros; - sensorValues[0] = ((current_height - oldHeightValue)/delta_t); - sensorValues[0] = (sensorValues[0] < - 6 || sensorValues[0] > 6)? sensorValues[0]:0; + sensorValues[0] = ((current_height - oldHeightValue)/delta_t_baro); + //sensorValues[0] = ((sensorValues[0] < - 2) || (sensorValues[0] > 2))? sensorValues[0]:0; oldHeightValue = current_height; sensorValues[0]*=BAROMETER_SCALE; break; default: + current_micros = clock_get_us(); + current_micros = current_micros/1000000; + last_micros = current_micros; sensorValues[ROLL] = 0; sensorValues[PITCH] = 0; @@ -153,6 +162,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) { @@ -193,9 +206,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) break; case PID_ID_BAROMETER: - float currentThrottle = rc_input.Throttle - 1500; - float velocity = (currentThrottle < - 10 || currentThrottle > 10 )? currentThrottle:0; - desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle)*BAROMETER_SCALE; + 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: @@ -209,9 +222,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 ---- */ @@ -299,16 +312,16 @@ 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); } } @@ -371,8 +384,8 @@ void pidRun(uint8_t ID) { pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]); - PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit + (int)VelocityCompensation); - + //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; @@ -552,7 +565,7 @@ void pidInit() void pidEproom(void) { - PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 254; + PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 200; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index 72ecfd2..ae62db4 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -315,6 +315,7 @@ void barometer_CalculatePTA(uint32_t D1, uint32_t D2) 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(); } @@ -504,16 +505,45 @@ float barometer_GetCurrentAltitude() ***********************************************************************/ float barometer_GetCurrentAveragedtAltitude() { - float toReturn = 0; - /* Go through all the values in the buffer */ - for (int i = 0; i < averageAltitudeCount; i++) - { - toReturn += averageAltitude[i]; - } +// 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; + - /* Return the average of the stored values */ - toReturn = toReturn/averageAltitudeCount; - return toReturn; } diff --git a/UAV-ControlSystem/src/drivers/beeper.c b/UAV-ControlSystem/src/drivers/beeper.c index 573c7dd..f6a156e 100644 --- a/UAV-ControlSystem/src/drivers/beeper.c +++ b/UAV-ControlSystem/src/drivers/beeper.c @@ -11,17 +11,17 @@ uint16_t beeperPin; GPIO_TypeDef* beeperPort; -void initBeeper(uint16_t led_pin, GPIO_TypeDef* led_port) +void initBeeper(uint16_t beeper_pin, GPIO_TypeDef* beeper_port) { - beeperPin = led_pin; - beeperPort = led_port; + beeperPin = beeper_pin; + beeperPort = beeper_port; GPIO_InitTypeDef gpinit; - gpinit.Pin = led_pin; - gpinit.Mode = GPIO_MODE_OUTPUT_OD; - gpinit.Pull = GPIO_NOPULL; + gpinit.Pin = beeper_pin; + gpinit.Mode = GPIO_MODE_OUTPUT_PP; + gpinit.Pull = GPIO_PULLUP; gpinit.Speed = GPIO_SPEED_HIGH; - HAL_GPIO_Init(led_port, &gpinit); + HAL_GPIO_Init(beeper_port, &gpinit); } diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index efea561..25ba171 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -74,30 +74,17 @@ typedef struct { */ static const motorMixer_s mixerUAV[] = { /* Throttle, Roll, Pitch, Yaw */ -// -// { 1.0f, 1.0f, 1.0f, -1.0f}, //M1 -// { 1.0f, 1.0f, 1.0f, 1.0f}, //M2 -// { 1.0f, 0.0f, 1.0f, 0.0f}, //M3 -// { 1.0f, 0.0f, 1.0f, 0.0f}, //M4 -// { 1.0f, -1.0f, 1.0f, 1.0f}, //M5 -// { 1.0f, -1.0f, 1.0f, -1.0f}, //M6 -// { 1.0f, 1.0f, -1.0f, 1.0f}, //M7 -// { 1.0f, 1.0f, -1.0f, -1.0f}, //M8 -// { 1.0f, -1.0f, -1.0f, -1.0f}, //M9 -// { 1.0f, -1.0f, -1.0f, 1.0f}, //M10 - - - { 1.0f, 1.0f, 1.0f, -1.0f}, //M1 - { 0.0f, 0.0f, 0.0f, 0.0f}, //M2 - { 0.0f, 0.0f, 0.0f, 0.0f}, //M3 - { 0.0f, 0.0f, 0.0f, 0.0f}, //M4 - { 0.0f, 0.0f, 0.0f, 0.0f}, //M5 - { 1.0f, -1.0f, 1.0f, -1.0f}, //M6 - { 0.0f, 0.0f, 0.0f, 0.0f}, //M7 - { 0.0f, 0.0f, 0.0f, 0.0f}, //M8 - { 0.0f, 0.0f, 0.0f, 0.0f}, //M9 - { 0.0f, 0.0f, 0.0f, 0.0f}, //M10 + { 1.0f, 1.0f, 1.0f, -1.0f}, //M1 + { 1.0f, 1.0f, 1.0f, 1.0f}, //M2 + { 1.0f, 0.0f, 1.0f, 0.0f}, //M3 + { 1.0f, 0.0f, 1.0f, 0.0f}, //M4 + { 1.0f, -1.0f, 1.0f, 1.0f}, //M5 + { 1.0f, -1.0f, 1.0f, -1.0f}, //M6 + { 1.0f, 1.0f, -1.0f, 1.0f}, //M7 + { 1.0f, 1.0f, -1.0f, -1.0f}, //M8 + { 1.0f, -1.0f, -1.0f, -1.0f}, //M9 + { 1.0f, -1.0f, -1.0f, 1.0f}, //M10 }; @@ -197,9 +184,9 @@ void mix() 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 is 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); + // 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 @@ -305,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]); }