From 83c11bd444c6c8332f47a508a26a80ac1818b85d Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 31 Oct 2016 18:19:44 +0100 Subject: [PATCH 01/33] Added miss to the acc calibration --- UAV-ControlSystem/src/tasks_main.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 4392405..624ded6 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -163,6 +163,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; @@ -180,23 +181,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; } } From 606160e1cf180e390af1ef222fc2a7b482db0394 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 1 Nov 2016 09:57:06 +0100 Subject: [PATCH 02/33] test --- UAV-ControlSystem/src/drivers/barometer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index b3fa766..9c07bab 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -262,7 +262,7 @@ void barometer_CaclulateValues() uint8_t cobuf[3] = {0}; uint32_t startTime; uint32_t endTime; - +//balbalblablalbalb //If the machine is armed and not calibrated we perform a calibraton if (!flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) From 88f09eef257488a414d8a49b59009597a6bfecdf Mon Sep 17 00:00:00 2001 From: philsson Date: Wed, 2 Nov 2016 09:22:22 +0100 Subject: [PATCH 03/33] First commit to new acc implementation --- UAV-ControlSystem/inc/drivers/accel_gyro.h | 3 + UAV-ControlSystem/src/Flight/pid.c | 71 +++++++++---------- UAV-ControlSystem/src/drivers/accel_gyro.c | 82 ++++++++++++++++++++++ 3 files changed, 117 insertions(+), 39 deletions(-) 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/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index c16c8ec..cee2fc7 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -65,6 +65,7 @@ float accPitchFineTune = 0; +//TODO: Remove as implemented in accel_gyro /************************************************************************** * BRIEF: Calculates angle from accelerometer * * INFORMATION: * @@ -151,56 +152,48 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_ACCELEROMETER: - mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ - -// -// if (calcGravity(accelProfile) > 1.15) -// { -// -// sensorValues[ROLL] = gyroProfile.gyroY*PidProfileBuff[ROLL].dT; -// sensorValues[PITCH] = gyroProfile.gyroX*PidProfileBuff[PITCH].dT; -// -// } -// else -// { + mpu6000_read_angle(&accelProfile,&gyroProfile); /*Reads data from accelerometer*/ float alpha = 0.5; /*May need Low pass filter since the accelerometer may drift*/ - float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); +// 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; +// X_roll += accRollFineTune; +// X_pitch += accPitchFineTune; + + sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune; + sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; - 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]; +// oldSensorValueRoll[i] = X_roll; +// oldSensorValuePitch[i] = X_pitch; // - oldSensorValue[0] = sensorValues[ROLL]; - oldSensorValue[1] = sensorValues[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); diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 759dcc5..e8c7e09 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -7,6 +7,8 @@ #include #include "drivers/spi.h" +#include "utilities.h" +#include "math.h" spi_profile mpu6000_spi_profile; uint8_t num_failed_receive = 0; @@ -514,3 +516,83 @@ bool mpu6000_who_am_i() return false; } + + +/* 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; + + static float lpf_Acc[3] = {0}; + static float smooth[3] = {0}; + + /* We read the accelerometer to get fresh data */ + mpu6000_read_accel(accel); + + /* Filter part */ + + for (int i = 0; i < 3; i ++) + smooth[i] = lpf_Acc[i] / 16; + + lpf_Acc[0] += sqrtf(accel->accelXconv) - smooth[0]; + lpf_Acc[1] += sqrtf(accel->accelYconv) - smooth[1]; + lpf_Acc[2] += sqrtf(accel->accelZconv) - smooth[2]; + + accel->accelXconv = smooth[0] * smooth[0]; + accel->accelYconv = smooth[1] * smooth[1]; + accel->accelZconv = smooth[2] * smooth[2]; + + /* The total magnitude of the acceleration */ + float magnitude = sqrtf( \ + ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + \ + ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + \ + ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) \ + ); + + /* Everything is normal. No outer forces */ + if (0.85 < magnitude && magnitude < 1.15) + { + + //TODO: JOHAN FIXAR! Kolla så det är rätt här hela vägen + // Roll + accel->rollAngle = atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; + + if (accel->rollAngle > 0) + { + if (accel->accelYconv < 0 ) + accel->rollAngle = 180 - accel->rollAngle; + } + else + { + if (accel->accelYconv < 0 ) + accel->rollAngle = - 180 - accel->rollAngle; + } + accel->rollAngle = -accel->rollAngle; + + + // Pitch + accel->pitchAngle = atan2( accel->accelYconv, sqrt(accel->accelZconv*accel->accelZconv + accel->accelXconv*accel->accelXconv))*180/M_PI; + + if (accel->pitchAngle > 0) + { + if (accel->accelYconv < 0) + accel->pitchAngle = 180 - accel->pitchAngle; + } + else + { + if (accel->accelYconv < 0 ) + accel->pitchAngle = - 180 - accel->pitchAngle; + } + + } + /* Too big forces to calculate on ACC data. Fallback on gyro integration */ + else + { + accel->rollAngle += (float)(delta_t * gyro->gyroX) / 1000000.0; + accel->pitchAngle += (float)(delta_t * gyro->gyroY) / 1000000.0; + } + +} From 29e621b18e06def7ed6bf8cf948697197bda51ee Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 2 Nov 2016 11:17:59 +0100 Subject: [PATCH 04/33] added new calcualtions to accel gyro --- UAV-ControlSystem/src/Flight/pid.c | 55 -------------------- UAV-ControlSystem/src/drivers/accel_gyro.c | 60 +++++++++------------- 2 files changed, 24 insertions(+), 91 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index cee2fc7..9b5d0cf 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -154,57 +154,9 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) mpu6000_read_angle(&accelProfile,&gyroProfile); /*Reads data from accelerometer*/ - - float alpha = 0.5; - /*May need Low pass filter since the accelerometer may drift*/ - -// 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; - sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune; sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; - -// 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); - break; case PID_ID_COMPASS: case PID_ID_BAROMETER: @@ -307,7 +259,6 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, 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 @@ -320,12 +271,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; diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index e8c7e09..f9c40c9 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -528,6 +528,7 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) static float lpf_Acc[3] = {0}; static float smooth[3] = {0}; + float sign[3] = {0}; /* We read the accelerometer to get fresh data */ mpu6000_read_accel(accel); @@ -535,15 +536,26 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) /* Filter part */ for (int i = 0; i < 3; i ++) + { smooth[i] = lpf_Acc[i] / 16; + } - lpf_Acc[0] += sqrtf(accel->accelXconv) - smooth[0]; - lpf_Acc[1] += sqrtf(accel->accelYconv) - smooth[1]; - lpf_Acc[2] += sqrtf(accel->accelZconv) - smooth[2]; + sign[0] = (accel->accelXconv< 0) ? -1 : 1; + lpf_Acc[0] += sign[0]*sqrtf(ABS_FLOAT(accel->accelXconv)) - smooth[0]; - accel->accelXconv = smooth[0] * smooth[0]; - accel->accelYconv = smooth[1] * smooth[1]; - accel->accelZconv = smooth[2] * smooth[2]; + + sign[1] = (accel->accelYconv< 0) ? -1 : 1; + lpf_Acc[1] += sign[1]*sqrtf(ABS_FLOAT(accel->accelYconv)) - smooth[1]; + + sign[2] = (accel->accelZconv< 0) ? -1 : 1; + lpf_Acc[2] += sign[2]*sqrtf(ABS_FLOAT(accel->accelZconv)) - smooth[2]; + + + + + accel->accelXconv = smooth[0] * smooth[0] * sign[0]; + accel->accelYconv = smooth[1] * smooth[1] * sign[1]; + accel->accelZconv = smooth[2] * smooth[2] * sign[2]; /* The total magnitude of the acceleration */ float magnitude = sqrtf( \ @@ -555,44 +567,20 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) /* Everything is normal. No outer forces */ if (0.85 < magnitude && magnitude < 1.15) { - - //TODO: JOHAN FIXAR! Kolla så det är rätt här hela vägen // Roll - accel->rollAngle = atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; - - if (accel->rollAngle > 0) - { - if (accel->accelYconv < 0 ) - accel->rollAngle = 180 - accel->rollAngle; - } - else - { - if (accel->accelYconv < 0 ) - accel->rollAngle = - 180 - accel->rollAngle; - } - accel->rollAngle = -accel->rollAngle; - + accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; // Pitch accel->pitchAngle = atan2( accel->accelYconv, sqrt(accel->accelZconv*accel->accelZconv + accel->accelXconv*accel->accelXconv))*180/M_PI; - - if (accel->pitchAngle > 0) - { - if (accel->accelYconv < 0) - accel->pitchAngle = 180 - accel->pitchAngle; - } - else - { - if (accel->accelYconv < 0 ) - accel->pitchAngle = - 180 - accel->pitchAngle; - } - } /* Too big forces to calculate on ACC data. Fallback on gyro integration */ else { - accel->rollAngle += (float)(delta_t * gyro->gyroX) / 1000000.0; - accel->pitchAngle += (float)(delta_t * gyro->gyroY) / 1000000.0; + accel->rollAngle += (float)(delta_t * gyro->gyroY) / 1000000.0; + accel->pitchAngle += (float)(delta_t * gyro->gyroX) / 1000000.0; + + accel->rollAngle = constrainf(accel->rollAngle, -90,90); + accel->pitchAngle = constrainf(accel->pitchAngle, -90,90); } } From 8bd9dc986d33693aa00e9c294a5830493fb5d721 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 2 Nov 2016 13:39:15 +0100 Subject: [PATCH 05/33] PID deleted rows --- UAV-ControlSystem/src/Flight/pid.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 9b5d0cf..ada9904 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -137,8 +137,6 @@ int i = 0; **************************************************************************/ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) { - - switch (ID_profile) { case PID_ID_GYRO: From 3eeb4d3741c2c680fa3730f8576c37709324eed8 Mon Sep 17 00:00:00 2001 From: philsson Date: Thu, 3 Nov 2016 11:58:51 +0100 Subject: [PATCH 06/33] Remove square root from magnitude calculation on accel_gyro.c Motors start now at 990 instead of 1000 to prevent glitches --- UAV-ControlSystem/src/drivers/accel_gyro.c | 7 +++++-- UAV-ControlSystem/src/drivers/motors.c | 7 ++++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index f9c40c9..bb567dc 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -245,6 +245,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel) HAL_Delay(60); + accel->pitchAngle = 0; + accel->rollAngle = 0; return true; } @@ -558,14 +560,15 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) accel->accelZconv = smooth[2] * smooth[2] * sign[2]; /* The total magnitude of the acceleration */ - float magnitude = sqrtf( \ + float magnitude = ( \ ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + \ ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + \ ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) \ ); /* Everything is normal. No outer forces */ - if (0.85 < magnitude && magnitude < 1.15) + if (0.81 < magnitude && magnitude < 1.21) + //if (false) { // Roll accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; 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); + } } /************************************************************************** From 280240815da22eec5639719d03a0caa9e56a4dfa Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 7 Nov 2016 10:43:43 +0100 Subject: [PATCH 07/33] Possible fix to the accelerometer problem --- UAV-ControlSystem/inc/utilities.h | 5 ++ UAV-ControlSystem/src/Flight/pid.c | 13 ---- UAV-ControlSystem/src/drivers/accel_gyro.c | 87 ++++++++++++---------- UAV-ControlSystem/src/tasks_main.c | 1 + UAV-ControlSystem/src/utilities.c | 14 ++++ 5 files changed, 68 insertions(+), 52 deletions(-) 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 ada9904..8274506 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -110,19 +110,6 @@ float convertData(int inputRange, int outputRange, int offset, float value) return ((float)outputRange/(float)inputRange)*(value-(float)offset); } -/************************************************************************** -* 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; -} float oldSensorValue[2] = {0}; diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index f9c40c9..dae9b97 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -9,6 +9,7 @@ #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; @@ -518,6 +519,17 @@ bool mpu6000_who_am_i() } + + + +/* 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^6) // that means a CMP_FACTOR of 1024 (2^10) + +#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) { @@ -526,61 +538,58 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) 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 */ - + /* 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] / 16; + + //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]); + } - sign[0] = (accel->accelXconv< 0) ? -1 : 1; - lpf_Acc[0] += sign[0]*sqrtf(ABS_FLOAT(accel->accelXconv)) - smooth[0]; + //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]; - sign[1] = (accel->accelYconv< 0) ? -1 : 1; - lpf_Acc[1] += sign[1]*sqrtf(ABS_FLOAT(accel->accelYconv)) - smooth[1]; - - sign[2] = (accel->accelZconv< 0) ? -1 : 1; - lpf_Acc[2] += sign[2]*sqrtf(ABS_FLOAT(accel->accelZconv)) - smooth[2]; - - - - - accel->accelXconv = smooth[0] * smooth[0] * sign[0]; - accel->accelYconv = smooth[1] * smooth[1] * sign[1]; - accel->accelZconv = smooth[2] * smooth[2] * sign[2]; - - /* The total magnitude of the acceleration */ - float magnitude = sqrtf( \ - ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + \ - ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + \ - ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) \ - ); - - /* Everything is normal. No outer forces */ - if (0.85 < magnitude && magnitude < 1.15) + //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) { - // Roll - accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; + //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; - // Pitch - accel->pitchAngle = atan2( accel->accelYconv, sqrt(accel->accelZconv*accel->accelZconv + accel->accelXconv*accel->accelXconv))*180/M_PI; - } - /* Too big forces to calculate on ACC data. Fallback on gyro integration */ - else - { - accel->rollAngle += (float)(delta_t * gyro->gyroY) / 1000000.0; - accel->pitchAngle += (float)(delta_t * gyro->gyroX) / 1000000.0; - - accel->rollAngle = constrainf(accel->rollAngle, -90,90); - accel->pitchAngle = constrainf(accel->pitchAngle, -90,90); + //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; } + //Always constran the angular values within the possible ranges 90 to -90 + accel->rollAngle = constrainf(accel->rollAngle, -90,90); + accel->pitchAngle = constrainf(accel->pitchAngle, -90,90); } diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 4392405..a3293ae 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -64,6 +64,7 @@ void systemTaskGyroPid(void) void systemTaskAccelerometer(void) { + flags_Set_ID(systemFlags_flightmode_acceleromter_id); pidRun(PID_ID_ACCELEROMETER); //update the accelerometer data // uint8_t c = 97; 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; +} From e22ad43c3926d7b15dc942f1f21532ee6d8ac82b Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 7 Nov 2016 13:42:43 +0100 Subject: [PATCH 08/33] Removed very very very small amount of stuff --- UAV-ControlSystem/src/Flight/pid.c | 1 - UAV-ControlSystem/src/tasks_main.c | 1 - 2 files changed, 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 8274506..6ef9db6 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -567,6 +567,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/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index a3293ae..4392405 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -64,7 +64,6 @@ void systemTaskGyroPid(void) void systemTaskAccelerometer(void) { - flags_Set_ID(systemFlags_flightmode_acceleromter_id); pidRun(PID_ID_ACCELEROMETER); //update the accelerometer data // uint8_t c = 97; From e09c69fbaab847c8001aaeb8178a944ff2019047 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 7 Nov 2016 16:57:59 +0100 Subject: [PATCH 09/33] small things --- UAV-ControlSystem/src/Flight/pid.c | 2 -- UAV-ControlSystem/src/config/cli.c | 12 ++++++------ UAV-ControlSystem/src/drivers/accel_gyro.c | 2 +- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 6ef9db6..d4cc9d5 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -558,8 +558,6 @@ void pidInit() void pidEproom(void) { - PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2; - PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 1d7cb04..5f5bee6 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 */ diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index bc019df..471f835 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -526,7 +526,7 @@ bool mpu6000_who_am_i() /* 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^6) // that means a CMP_FACTOR of 1024 (2^10) +#define GYRO_ACC_DIV_FACTOR (2^8) // that means a CMP_FACTOR of 1024 (2^10) #define GetMagnitude(x) (x*x) #define Low_Magnitude (GetMagnitude(0.85)) From 70b70fc5b63b8d26ea29da4ce2c205b9af20fde7 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 7 Nov 2016 16:58:07 +0100 Subject: [PATCH 10/33] ops --- UAV-ControlSystem/src/Flight/pid.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index d4cc9d5..13bd3e9 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -558,6 +558,8 @@ void pidInit() void pidEproom(void) { + PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 4; + PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 4; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms From fcc0bd41ae4606a0966e698901c74a8a39ca2bff Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 8 Nov 2016 09:25:52 +0100 Subject: [PATCH 11/33] Added some comments. Added some comments to the barometer code. More will be added. --- UAV-ControlSystem/inc/drivers/barometer.h | 34 +++++++- UAV-ControlSystem/src/drivers/barometer.c | 99 ++++++++++++++--------- 2 files changed, 94 insertions(+), 39 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/barometer.h b/UAV-ControlSystem/inc/drivers/barometer.h index 2c47476..41e9172 100644 --- a/UAV-ControlSystem/inc/drivers/barometer.h +++ b/UAV-ControlSystem/inc/drivers/barometer.h @@ -8,18 +8,50 @@ #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 + ***********************************************************************/ bool barometer_reset(); +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ void barometer_CaclulateValues(); +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ double barometer_GetCurrentPreassure(); +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ double barometer_GetCurrentTemperature(); -float barometer_GetCurrentAltitudeBasedOnSeaLevel(); +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ +float barometer_GetCurrentAltitude(); diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index 9c07bab..d9c7660 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -13,7 +13,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 +31,37 @@ #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 -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) - -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; - -//TODO: remove when not used for testing any more -uint32_t tempTestCounterStart = 0; - -uint8_t cobuf[3] = {0}; +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 /* 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: + * INFORMATION: + ***********************************************************************/ void barometer_addCalibrationSample() { //fisrt check if the amount of samples is greater than the array @@ -80,6 +79,10 @@ void barometer_addCalibrationSample() calibrationSamplesIterator ++; } +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ bool barometer_Calibrate() { //Check if any calibration values exist @@ -108,6 +111,10 @@ bool barometer_Calibrate() return true; } +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ bool barometer_init() { //Set the sample rate of the data that will be calculated on the barometer peripheral @@ -118,12 +125,16 @@ bool barometer_init() i2c_soft_Init(I2C1, &baroI2C_soft_handle); #endif #ifdef BARO_USE_I2C_HARD - + //Todo: needs implementation #endif return true; } +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ bool barometer_reset() { /* Send a reset command to the baromter @@ -142,6 +153,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 +206,17 @@ 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: + * INFORMATION: + ***********************************************************************/ void barometer_CalculatePTA(uint32_t D1, uint32_t D2) { /* calculate dT, difference between actual and reference temp: (D2 - C5 * 2^8) */ @@ -247,6 +256,10 @@ void barometer_CalculatePTA(uint32_t D1, uint32_t D2) baro_Altitude = (flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) ? (feet * FTMETERS) - altitudeCalibrationValue : (feet * FTMETERS); } +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ void barometer_CaclulateValues() { /*the calculation is in need of different states. This is because the @@ -254,7 +267,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; @@ -262,7 +276,6 @@ void barometer_CaclulateValues() uint8_t cobuf[3] = {0}; uint32_t startTime; uint32_t endTime; -//balbalblablalbalb //If the machine is armed and not calibrated we perform a calibraton if (!flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) @@ -273,8 +286,6 @@ void barometer_CaclulateValues() } } - - switch (currentCalculationState) { case CALCSTATE_D2_CALCULATION: @@ -365,17 +376,29 @@ void barometer_CaclulateValues() } +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ double barometer_GetCurrentPreassure() { return baro_Preassure; } +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ double barometer_GetCurrentTemperature() { return baro_Temperature; } -float barometer_GetCurrentAltitudeBasedOnSeaLevel() +/*********************************************************************** + * BRIEF: + * INFORMATION: + ***********************************************************************/ +float barometer_GetCurrentAltitude() { return baro_Altitude; } From 9024a8d425d0b54cc00e57f0a52c841b1f7b430b Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 8 Nov 2016 12:26:07 +0100 Subject: [PATCH 12/33] Added a full set of comments to the barometer code. Fully commented the code for the barometer. --- UAV-ControlSystem/inc/drivers/barometer.h | 92 +++++++++++++----- UAV-ControlSystem/inc/drivers/i2c_soft.h | 41 ++++++-- UAV-ControlSystem/src/drivers/barometer.c | 111 +++++++++++++++++----- UAV-ControlSystem/src/drivers/i2c_soft.c | 62 ++++++++++++ 4 files changed, 251 insertions(+), 55 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/barometer.h b/UAV-ControlSystem/inc/drivers/barometer.h index 41e9172..7ce3b75 100644 --- a/UAV-ControlSystem/inc/drivers/barometer.h +++ b/UAV-ControlSystem/inc/drivers/barometer.h @@ -1,9 +1,24 @@ -/* - * 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_ @@ -17,42 +32,75 @@ typedef enum { }calculationState; /*********************************************************************** - * BRIEF: Initializes the barometer. - * INFORMATION: Initializes the barometer and it needs to be called - * before anything else when using the barometer. + * 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 + * 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: - * INFORMATION: + * 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(); /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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_GetCurrentPreassure(); +double barometer_GetCurrentPressure(); /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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(); /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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(); - - #endif /* DRIVERS_BAROMETER_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/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index d9c7660..5fd24a5 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -1,9 +1,23 @@ -/* - * barometer.c - * - * Created on: 18 okt. 2016 - * Author: holmis - */ +/************************************************************************** +* * +* 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" @@ -59,8 +73,9 @@ uint32_t coefficients_arr[8]; //coefficient storage uint8_t cobuf[3] = {0}; //Array used when writing and reading data over the I2C /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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() { @@ -80,8 +95,14 @@ void barometer_addCalibrationSample() } /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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() { @@ -112,8 +133,9 @@ bool barometer_Calibrate() } /*********************************************************************** - * BRIEF: - * INFORMATION: + * BRIEF: Initializes the barometer. * + * INFORMATION: Initializes the barometer and it needs to be called * + * before anything else when using the barometer. * ***********************************************************************/ bool barometer_init() { @@ -132,8 +154,12 @@ bool barometer_init() } /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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() { @@ -214,8 +240,12 @@ bool barometer_reset() } /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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) { @@ -257,8 +287,30 @@ void barometer_CalculatePTA(uint32_t D1, uint32_t D2) } /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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() { @@ -377,17 +429,23 @@ void barometer_CaclulateValues() } /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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_GetCurrentPreassure() +double barometer_GetCurrentPressure() { return baro_Preassure; } /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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() { @@ -395,8 +453,11 @@ double barometer_GetCurrentTemperature() } /*********************************************************************** - * BRIEF: - * INFORMATION: + * 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() { 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 From d7406cc5015c63ab5736672be1a354a0167327c1 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 8 Nov 2016 12:26:19 +0100 Subject: [PATCH 13/33] ops --- UAV-ControlSystem/src/drivers/barometer.c | 1 + 1 file changed, 1 insertion(+) diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index 5fd24a5..798d71a 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -1,4 +1,5 @@ /************************************************************************** +* NAME: barometer.c * * * * AUTHOR: Jonas Holmberg * * * From 9d1b71fabc6b192616e4ba9eaf44aeebaeb0a6d1 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 9 Nov 2016 08:35:37 +0100 Subject: [PATCH 14/33] PID update, deleted code overflow --- UAV-ControlSystem/src/Flight/pid.c | 45 ------------------------------ 1 file changed, 45 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 13bd3e9..3503841 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -64,43 +64,6 @@ float accRollFineTune = 0; float accPitchFineTune = 0; - -//TODO: Remove as implemented in accel_gyro -/************************************************************************** -* BRIEF: Calculates angle from accelerometer * -* INFORMATION: * -**************************************************************************/ -float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis) -{ - float angle; - - switch (axis) - { - case ROLL: - - angle = atan2(x_axis, sqrt(y_axis*y_axis + z_axis*z_axis))*180/M_PI; - angle = -1*((angle > 0)? (z_axis < 0 )? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle); - - break; - case PITCH: - - angle = atan2( y_axis, sqrt(z_axis*z_axis + x_axis*x_axis))*180/M_PI; /*down (the front down against ground) = pos angle*/ - angle = (angle > 0)? ((z_axis < 0))? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle; - - break; - default: - angle = 0; - break; - } - - return angle; -} - -float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, const float z_axis) -{ - return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv); -} - /************************************************************************** * BRIEF: Scales data from input range to output range * * INFORMATION: * @@ -110,14 +73,6 @@ float convertData(int inputRange, int outputRange, int offset, float value) return ((float)outputRange/(float)inputRange)*(value-(float)offset); } - - -float oldSensorValue[2] = {0}; -float oldSensorValueRoll[12] = {0}; -float oldSensorValuePitch[12] = {0}; - -int i = 0; - /************************************************************************** * BRIEF: Update current sensor values * * INFORMATION: * From c791a9ea3bce410735f8d3efbbeed9f6b41fd123 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 9 Nov 2016 10:34:03 +0100 Subject: [PATCH 15/33] PID added barometer settings to CLI, eeprom and PID --- UAV-ControlSystem/inc/config/eeprom.h | 2 ++ UAV-ControlSystem/src/Flight/pid.c | 24 ++++++++++--- UAV-ControlSystem/src/config/cli.c | 52 +++++++++++++++++++++++++++ UAV-ControlSystem/src/config/eeprom.c | 10 ++++++ 4 files changed, 84 insertions(+), 4 deletions(-) diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 1774a35..7c1530c 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -208,6 +208,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/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 3503841..162ceed 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -27,7 +27,7 @@ #define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/ #define RADIO_RANGE 500 /*Radio range input*/ -#define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/ +#define BAROMETER_RANGE 3 /*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 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)*/ @@ -60,6 +60,7 @@ gyro_t gyroProfile = {0}; /*Struct profile for input data from senso pt1Filter_t accelFilter[2] = {0}; +float throttleRate = 0; float accRollFineTune = 0; float accPitchFineTune = 0; @@ -97,9 +98,22 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune; sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; + /*Checks the biggest angle */ + throttleRate = 2 - (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? cos(sensorValues[ROLL]*M_PI/180) : cos(sensorValues[PITCH]*M_PI/180); + break; case PID_ID_COMPASS: + + sensorValues[ROLL] = 0; + sensorValues[PITCH] = 0; + sensorValues[YAW] = 0; + + + break; case PID_ID_BAROMETER: + + + break; default: sensorValues[ROLL] = 0; @@ -157,7 +171,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); + //desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate); + + desiredCommand[THROTTLE] = 1.9; break; default: @@ -324,9 +340,9 @@ 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; + PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle*throttleRate; } else { diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 5f5bee6..d77f8a9 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -284,9 +284,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, @@ -659,6 +672,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_ACCEL_I_PITCH] = + { + "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_ACCEL_D_PITCH] = + { + "pid_baro_d_height", COMMAND_ID_PID_ACCEL_D_PITCH, 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] = { @@ -668,6 +716,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 7852cc1..d727865 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -291,6 +291,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 */ From 440640d8ef5fac5c465577ca459178eca8d41ef3 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Wed, 9 Nov 2016 10:35:55 +0100 Subject: [PATCH 16/33] Added average function to get the altitude --- UAV-ControlSystem/inc/drivers/barometer.h | 7 +++ UAV-ControlSystem/src/drivers/barometer.c | 54 ++++++++++++++++++++++- UAV-ControlSystem/src/main.c | 4 +- UAV-ControlSystem/src/tasks_main.c | 6 +-- 4 files changed, 64 insertions(+), 7 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/barometer.h b/UAV-ControlSystem/inc/drivers/barometer.h index 7ce3b75..a22b488 100644 --- a/UAV-ControlSystem/inc/drivers/barometer.h +++ b/UAV-ControlSystem/inc/drivers/barometer.h @@ -103,4 +103,11 @@ double barometer_GetCurrentTemperature(); ***********************************************************************/ 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/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index 798d71a..3074445 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -49,7 +49,8 @@ #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; //Handle for the HW i2c (NOT USED ATM) DMA_HandleTypeDef baroI2C_Rx_DMA_handle; //Dma handle receive (NOT USED ATM) @@ -62,17 +63,46 @@ double baro_Temperature; // compensated temperature value ( double baro_Altitude; // altitude 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 +/* average altitude variables */ +float averageAltitude[NUMB_AVERAGE_VALS]; +uint8_t averageAltitudeIterator = 0; +uint8_t averageAltitudeCount = 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 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 * @@ -283,8 +313,10 @@ 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(); } /*********************************************************************** @@ -465,4 +497,22 @@ 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 */ + return toReturn/averageAltitudeCount; +} + diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 2c9dfaf..4441435 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -77,8 +77,8 @@ void init_system() #endif #ifdef BARO - //barometer_init(); - //barometer_reset(); + barometer_init(); + barometer_reset(); #endif #ifdef COMPASS diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 624ded6..8f199d9 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -112,7 +112,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) @@ -234,7 +233,7 @@ void systemTaskBattery(void) void systemTaskBaro(void) { - //barometer_CaclulateValues(); + barometer_CaclulateValues(); } void systemTaskCompass(void) @@ -258,7 +257,8 @@ void systemTaskAltitude(void) //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); From f49548d1419f3daa15b88966fa749f0cea15c2c2 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Wed, 9 Nov 2016 12:06:32 +0100 Subject: [PATCH 17/33] Merged with accelerometer. Started baro implementation, not working. --- UAV-ControlSystem/inc/system_variables.h | 2 +- UAV-ControlSystem/src/Flight/pid.c | 38 +++++++++---------- UAV-ControlSystem/src/config/cli.c | 6 +-- .../src/drivers/failsafe_toggles.c | 6 +-- UAV-ControlSystem/src/tasks_main.c | 2 +- 5 files changed, 26 insertions(+), 28 deletions(-) diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h index 0217fd9..28da8fe 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 107 +#define EEPROM_SYS_VERSION 109 #define ADC_STATE #include "stm32f4xx.h" diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 162ceed..471b1db 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -60,7 +60,7 @@ gyro_t gyroProfile = {0}; /*Struct profile for input data from senso pt1Filter_t accelFilter[2] = {0}; -float throttleRate = 0; +float throttleRate = 1; float accRollFineTune = 0; float accPitchFineTune = 0; @@ -112,7 +112,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_BAROMETER: - + sensorValues[0] = barometer_GetCurrentAveragedtAltitude(); break; default: @@ -340,7 +340,8 @@ void pidRun(uint8_t ID) break; case PID_ID_BAROMETER: - if (!PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) + + if (!(PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id))) { PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle*throttleRate; } @@ -442,6 +443,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; @@ -457,16 +462,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] = 4; + PidProfile[ID].PIDweight[PITCH] = 4; PidProfile[ID].PIDweight[YAW] = 100; PidProfile[ID].pidEnabled = true; @@ -477,27 +482,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 = 1000; break; default: @@ -528,9 +527,8 @@ void pidInit() void pidEproom(void) { + PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 200; - PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 4; - PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 4; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index d77f8a9..2c99737 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -679,14 +679,14 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { }, //BARO I - [COMMAND_ID_PID_ACCEL_I_PITCH] = + [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_ACCEL_D_PITCH] = + [COMMAND_ID_PID_BARO_D_HEIGHT] = { - "pid_baro_d_height", COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 255} + "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 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/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 8f199d9..f5c3b07 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -258,7 +258,7 @@ void systemTaskAltitude(void) //double temperature = barometer_GetCurrentTemperature(); //double pressure = barometer_GetCurrentPreassure(); //float altitute = barometer_GetCurrentAltitude(); - float altitute = barometer_GetCurrentAveragedtAltitude(); + //float altitute = barometer_GetCurrentAveragedtAltitude(); //pid run, should probably be moved to systemTaskAltitude pidRun(PID_ID_BAROMETER); From 38486e2bd32bfb2d1e25ffe8383215663c95c28c Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Wed, 9 Nov 2016 16:26:28 +0100 Subject: [PATCH 18/33] Seems to be a working with the barometer in the lab. It looks like it could hold the altitude. Testing outside is needed to verify and tune. --- UAV-ControlSystem/src/Flight/pid.c | 9 +++++---- UAV-ControlSystem/src/drivers/barometer.c | 3 ++- UAV-ControlSystem/src/drivers/motormix.c | 7 ++++++- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 471b1db..ad5a3ee 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -20,6 +20,7 @@ #include "drivers/failsafe_toggles.h" #include "drivers/motormix.h" #include "utilities.h" +#include "drivers/barometer.h" #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ @@ -112,7 +113,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_BAROMETER: - sensorValues[0] = barometer_GetCurrentAveragedtAltitude(); + sensorValues[0] = barometer_GetCurrentAveragedtAltitude()*20; break; default: @@ -173,7 +174,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) //desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate); - desiredCommand[THROTTLE] = 1.9; + desiredCommand[THROTTLE] = 1*20; break; default: @@ -496,7 +497,7 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) PidProfile[ID].pidEnabled = false; PidProfile[ID].dterm_lpf = 90; - PidProfile[ID].pid_out_limit = 1000; + PidProfile[ID].pid_out_limit = 2000; break; default: @@ -527,7 +528,7 @@ void pidInit() void pidEproom(void) { - PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 200; + PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 254; PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c index 3074445..72ecfd2 100644 --- a/UAV-ControlSystem/src/drivers/barometer.c +++ b/UAV-ControlSystem/src/drivers/barometer.c @@ -512,7 +512,8 @@ float barometer_GetCurrentAveragedtAltitude() } /* Return the average of the stored values */ - return toReturn/averageAltitudeCount; + toReturn = toReturn/averageAltitudeCount; + return toReturn; } diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 0def06d..aff2051 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -103,7 +103,12 @@ 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]; + + if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) + { + throttle += 1482; + } /* Mixer Full Scale enabled */ From 02e33f7d33a6d9296bef88644d8b0046a50c196a Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 9 Nov 2016 16:56:56 +0100 Subject: [PATCH 19/33] PID and Motormix, code optimization --- UAV-ControlSystem/src/Flight/pid.c | 5 +++-- UAV-ControlSystem/src/drivers/motormix.c | 6 +----- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index ad5a3ee..6288df7 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -26,6 +26,7 @@ #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 20 #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 3 /*Determines the range of the maximum height (limits the rc input)*/ @@ -113,7 +114,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_BAROMETER: - sensorValues[0] = barometer_GetCurrentAveragedtAltitude()*20; + sensorValues[0] = barometer_GetCurrentAveragedtAltitude()*BAROMETER_SCALE; break; default: @@ -174,7 +175,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) //desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate); - desiredCommand[THROTTLE] = 1*20; + desiredCommand[THROTTLE] = 1*BAROMETER_SCALE; break; default: diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index aff2051..88a1778 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -105,11 +105,7 @@ void mix() int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]; - if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) - { - throttle += 1482; - } - + if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += 1482; /* Mixer Full Scale enabled */ if (flags_IsSet_ID(systemFlags_mixerfullscale_id)) From ce6ca20b43d2f8a5e981cce3cd73ce286b9cb465 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 10 Nov 2016 10:31:26 +0100 Subject: [PATCH 20/33] PID test flight baro --- UAV-ControlSystem/src/Flight/pid.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index ad5a3ee..4acc068 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -174,7 +174,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) //desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate); - desiredCommand[THROTTLE] = 1*20; + desiredCommand[THROTTLE] = 3*20; break; default: @@ -349,6 +349,8 @@ void pidRun(uint8_t ID) 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], -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit); + } break; From aa93a954d89b4f7fd73eb6ffd2bd4611322ed13b Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 10 Nov 2016 13:30:24 +0100 Subject: [PATCH 21/33] Fixed throttlerate, added constrain so that we cant lower altitude to fast with barometer flight. --- UAV-ControlSystem/inc/Flight/pid.h | 5 ++++- UAV-ControlSystem/src/Flight/pid.c | 11 ++++++----- UAV-ControlSystem/src/drivers/motormix.c | 4 ++-- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index 7213f7d..ac014ac 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*/ diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index d256c48..0bf95bc 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -62,10 +62,11 @@ gyro_t gyroProfile = {0}; /*Struct profile for input data from senso pt1Filter_t accelFilter[2] = {0}; -float throttleRate = 1; float accRollFineTune = 0; float accPitchFineTune = 0; +float throttleRate = 1; +int HoverForce = 1485;/*Struct profile for input data from sensor*/ /************************************************************************** * BRIEF: Scales data from input range to output range * @@ -101,7 +102,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; /*Checks the biggest angle */ - throttleRate = 2 - (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? cos(sensorValues[ROLL]*M_PI/180) : cos(sensorValues[PITCH]*M_PI/180); + 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: @@ -175,7 +176,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) //desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate); - desiredCommand[THROTTLE] = 1*BAROMETER_SCALE; + desiredCommand[THROTTLE] = 5*BAROMETER_SCALE; break; default: @@ -345,12 +346,12 @@ void pidRun(uint8_t ID) if (!(PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id))) { - PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle*throttleRate; + 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], -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit); + PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -1, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); } diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 88a1778..124a50f 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -103,9 +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]; + 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 += 1482; + 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)) From 5873b4326e4a70c4fc750da991ee77d1739cf4c5 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Thu, 10 Nov 2016 14:12:59 +0100 Subject: [PATCH 22/33] PID added desired height value to define --- UAV-ControlSystem/src/Flight/pid.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 0bf95bc..6ca9571 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -37,6 +37,8 @@ #define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/ +#define DESIRED_HEIGHT 5 + /*Struct that belongs to a certain PID controller*/ typedef struct pidProfileBuff_s { @@ -176,7 +178,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) //desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate); - desiredCommand[THROTTLE] = 5*BAROMETER_SCALE; + desiredCommand[THROTTLE] = DESIRED_HEIGHT*BAROMETER_SCALE; break; default: From a5a888f5534ed81309a8b8f8bb8437739269a511 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Fri, 11 Nov 2016 11:01:43 +0100 Subject: [PATCH 23/33] Small value changes before second barometer test flight --- UAV-ControlSystem/src/Flight/pid.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 6ca9571..2c7844e 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -37,7 +37,7 @@ #define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/ -#define DESIRED_HEIGHT 5 +#define DESIRED_HEIGHT 1.5 /*Height value in meters*/ /*Struct that belongs to a certain PID controller*/ typedef struct pidProfileBuff_s { @@ -68,7 +68,7 @@ float accRollFineTune = 0; float accPitchFineTune = 0; float throttleRate = 1; -int HoverForce = 1485;/*Struct profile for input data from sensor*/ +int HoverForce = 1475;/*Struct profile for input data from sensor*/ /************************************************************************** * BRIEF: Scales data from input range to output range * @@ -353,7 +353,7 @@ void pidRun(uint8_t ID) 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], -1, (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); } From 4a1ac00dc12cd0fdd8d2d7065511ce9dcb62dc6d Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Fri, 11 Nov 2016 14:24:51 +0100 Subject: [PATCH 24/33] Changes made during testflight --- UAV-ControlSystem/src/Flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 2c7844e..8c644f1 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -37,7 +37,7 @@ #define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/ -#define DESIRED_HEIGHT 1.5 /*Height value in meters*/ +#define DESIRED_HEIGHT 5 /*Height value in meters*/ /*Struct that belongs to a certain PID controller*/ typedef struct pidProfileBuff_s { From 84b3a729e1c14a75d69eedf7467387256c6d2db9 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Fri, 11 Nov 2016 17:49:01 +0100 Subject: [PATCH 25/33] PID baro fix loosing heoight to fast --- UAV-ControlSystem/src/Flight/pid.c | 43 +++++++++++++++++++++++++++--- 1 file changed, 40 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 8c644f1..59ad95c 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -26,7 +26,7 @@ #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 20 +#define BAROMETER_SCALE 11 #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 3 /*Determines the range of the maximum height (limits the rc input)*/ @@ -79,12 +79,19 @@ float convertData(int inputRange, int outputRange, int offset, float value) return ((float)outputRange/(float)inputRange)*(value-(float)offset); } +uint8_t FlagVelocityLimit = 0; +float VelocityCompensation = 0; + /************************************************************************** * BRIEF: Update current sensor values * * INFORMATION: * **************************************************************************/ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) { + static float last_micros = 0; + static float oldHeightValue = 0; + static uint8_t counter = 0; + switch (ID_profile) { case PID_ID_GYRO: @@ -117,7 +124,36 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_BAROMETER: - sensorValues[0] = barometer_GetCurrentAveragedtAltitude()*BAROMETER_SCALE; + sensorValues[0] = barometer_GetCurrentAveragedtAltitude(); + + counter += 1; + + if (counter > 2) + { + + float current_micros = clock_get_us()/1000000; + float delta_t = (current_micros - last_micros); + last_micros = current_micros; + counter = 0; + + float current_velocity = (sensorValues[0] - oldHeightValue)/delta_t; + + if( current_velocity<= -10) + { + + VelocityCompensation = ABS_FLOAT(current_velocity*3); + + } + else + { + VelocityCompensation = 0; + } + + } + + oldHeightValue = sensorValues[0]; + sensorValues[0]*=BAROMETER_SCALE; + break; default: @@ -353,7 +389,8 @@ void pidRun(uint8_t ID) 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], -15, (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 + (int)VelocityCompensation); } From 7333665fc62c0405d7bddcfc758b3aa4dc8b9d7e Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 15 Nov 2016 09:13:47 +0100 Subject: [PATCH 26/33] Added small fixes, also added beeber. Beeber dont seem to work needs fixing. --- UAV-ControlSystem/inc/Scheduler/scheduler.h | 4 +-- UAV-ControlSystem/inc/config/eeprom.h | 2 +- UAV-ControlSystem/inc/drivers/beeper.h | 19 ++++++++++ UAV-ControlSystem/inc/stm32f4xx_revo.h | 5 +-- UAV-ControlSystem/inc/system_variables.h | 2 +- UAV-ControlSystem/src/Scheduler/scheduler.c | 2 +- UAV-ControlSystem/src/config/cli.c | 4 +-- UAV-ControlSystem/src/config/eeprom.c | 2 +- UAV-ControlSystem/src/drivers/beeper.c | 34 ++++++++++++++++++ UAV-ControlSystem/src/drivers/motormix.c | 39 +++++++++++++++------ UAV-ControlSystem/src/main.c | 8 +++-- UAV-ControlSystem/src/tasks_main.c | 1 + 12 files changed, 99 insertions(+), 23 deletions(-) create mode 100644 UAV-ControlSystem/inc/drivers/beeper.h create mode 100644 UAV-ControlSystem/src/drivers/beeper.c diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index 945f953..b6f5797 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -122,8 +122,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 7c1530c..5570534 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -174,7 +174,7 @@ typedef enum { #if defined(BARO) || defined(SONAR) EEPROM_PERIOD_ALTITUDE, #endif -#if BEEPER +#ifdef BEEPER EEPROM_PERIOD_BEEPER, #endif 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/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index 90f7cf2..b8de8ad 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 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 28da8fe..eac6e2f 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 109 +#define EEPROM_SYS_VERSION 110 #define ADC_STATE #include "stm32f4xx.h" diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index 340687a..beb4b26 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -359,7 +359,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 2c99737..63a62d2 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -198,7 +198,7 @@ typedef enum #if defined(BARO) || defined(SONAR) COMMAND_ID_PERIOD_ALTITUDE, #endif -#if BEEPER +#ifdef BEEPER COMMAND_ID_PERIOD_BEEPER, #endif @@ -414,7 +414,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} diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index d727865..bb5e39b 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -189,7 +189,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), diff --git a/UAV-ControlSystem/src/drivers/beeper.c b/UAV-ControlSystem/src/drivers/beeper.c new file mode 100644 index 0000000..573c7dd --- /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 led_pin, GPIO_TypeDef* led_port) +{ + beeperPin = led_pin; + beeperPort = led_port; + + GPIO_InitTypeDef gpinit; + gpinit.Pin = led_pin; + gpinit.Mode = GPIO_MODE_OUTPUT_OD; + gpinit.Pull = GPIO_NOPULL; + gpinit.Speed = GPIO_SPEED_HIGH; + HAL_GPIO_Init(led_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/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 124a50f..efea561 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -74,17 +74,30 @@ 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 - { 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 }; @@ -183,7 +196,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 - motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax); + + // 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); + } else // Mixer full scale NOT active { diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index cb10d8f..4a1317b 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -29,6 +29,7 @@ #include "drivers/motors.h" #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 @@ -96,8 +97,8 @@ void init_system() #endif -#if BEEPER - +#ifdef BEEPER + initBeeper(BEEPER_PIN, BEEPER_PORT); #endif @@ -120,6 +121,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 f5c3b07..e596fa9 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -42,6 +42,7 @@ #include "drivers/motormix.h" #include "Flight/pid.h" #include "drivers/barometer.h" +#include "drivers/beeper.h" void systemTaskGyroPid(void) { From 497e00aa2f0cfb321b068186b4b4d24ad2b2e699 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Tue, 15 Nov 2016 09:15:15 +0100 Subject: [PATCH 27/33] Added velocity compensation to baromter loop instead of controling height --- UAV-ControlSystem/src/Flight/pid.c | 41 ++++++++---------------------- 1 file changed, 11 insertions(+), 30 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 59ad95c..b9dd38c 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -29,7 +29,7 @@ #define BAROMETER_SCALE 11 #define RADIO_RANGE 500 /*Radio range input*/ -#define BAROMETER_RANGE 3 /*Determines the range of the maximum height (limits the rc input)*/ +#define BAROMETER_RANGE 10 /*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 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)*/ @@ -90,7 +90,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) { static float last_micros = 0; static float oldHeightValue = 0; - static uint8_t counter = 0; switch (ID_profile) { @@ -124,34 +123,16 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_BAROMETER: - sensorValues[0] = barometer_GetCurrentAveragedtAltitude(); + float current_micros = clock_get_us()/1000000; + float delta_t = (current_micros - last_micros); + float current_height = barometer_GetCurrentAveragedtAltitude(); - counter += 1; + last_micros = current_micros; - if (counter > 2) - { + sensorValues[0] = ((current_height - oldHeightValue)/delta_t); + sensorValues[0] = (sensorValues[0] < - 6 || sensorValues[0] > 6)? sensorValues[0]:0; - float current_micros = clock_get_us()/1000000; - float delta_t = (current_micros - last_micros); - last_micros = current_micros; - counter = 0; - - float current_velocity = (sensorValues[0] - oldHeightValue)/delta_t; - - if( current_velocity<= -10) - { - - VelocityCompensation = ABS_FLOAT(current_velocity*3); - - } - else - { - VelocityCompensation = 0; - } - - } - - oldHeightValue = sensorValues[0]; + oldHeightValue = current_height; sensorValues[0]*=BAROMETER_SCALE; break; @@ -212,9 +193,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*throttleRate); - - desiredCommand[THROTTLE] = DESIRED_HEIGHT*BAROMETER_SCALE; + 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; break; default: From 3407813f1e14d6b7616153cb75b154147cecf23a Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 17 Nov 2016 10:18:39 +0100 Subject: [PATCH 28/33] Added filter to the barometer data. Also some fixes in PID for barometer. Also added simple beeper code. --- UAV-ControlSystem/inc/stm32f4xx_revo.h | 2 +- UAV-ControlSystem/inc/system_variables.h | 2 +- UAV-ControlSystem/src/Flight/pid.c | 51 ++++++++++++++--------- UAV-ControlSystem/src/drivers/barometer.c | 48 +++++++++++++++++---- UAV-ControlSystem/src/drivers/beeper.c | 14 +++---- UAV-ControlSystem/src/drivers/motormix.c | 46 +++++++++----------- 6 files changed, 100 insertions(+), 63 deletions(-) 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]); } From 03be1e6b40f45f22c30ded815d4d26a747c315ec Mon Sep 17 00:00:00 2001 From: johan9107 Date: Thu, 17 Nov 2016 10:59:23 +0100 Subject: [PATCH 29/33] Deleted oscilation when enableing acc --- UAV-ControlSystem/src/Flight/pid.c | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 31027e9..18ceb86 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -325,6 +325,27 @@ void pidUAV(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff) } } +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 > 50) + { + PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll; + PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; + } + else + { + counterAcc +=1; + } +} + /************************************************************************** * BRIEF: Runs a certain PID Controller * * INFORMATION: * @@ -354,10 +375,12 @@ 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; } else { - pidUAV(&PidProfile[PID_ID_ACCELEROMETER], &PidProfileBuff[PID_ID_ACCELEROMETER]); + pidAccelerometer(); + flagAccBuff = 1; } break; From 2a87aa499eb2c2e3988cbdd3a339ca9a757e25f1 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Fri, 18 Nov 2016 09:29:10 +0100 Subject: [PATCH 30/33] Last flight settings --- UAV-ControlSystem/src/Flight/pid.c | 18 ++++++++++-------- UAV-ControlSystem/src/drivers/accel_gyro.c | 2 +- UAV-ControlSystem/src/drivers/motormix.c | 2 +- UAV-ControlSystem/src/tasks_main.c | 6 ++++++ 4 files changed, 18 insertions(+), 10 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 18ceb86..fbb1f94 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -26,7 +26,7 @@ #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*/ @@ -335,15 +335,13 @@ void pidAccelerometer(void) pidUAV(&PidProfile[PID_ID_ACCELEROMETER], &PidProfileBuff[PID_ID_ACCELEROMETER]); - if (counterAcc > 50) + if (counterAcc < 80) { PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll; PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; - } - else - { counterAcc +=1; } + } /************************************************************************** @@ -406,9 +404,11 @@ void pidRun(uint8_t ID) 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); + //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; @@ -531,8 +531,8 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) PidProfile[ID].D[PITCH] = 0; PidProfile[ID].D[YAW] = 0; - PidProfile[ID].PIDweight[ROLL] = 4; - PidProfile[ID].PIDweight[PITCH] = 4; + PidProfile[ID].PIDweight[ROLL] = 100; + PidProfile[ID].PIDweight[PITCH] = 100; PidProfile[ID].PIDweight[YAW] = 100; PidProfile[ID].pidEnabled = true; @@ -590,6 +590,8 @@ void pidEproom(void) { PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 200; + 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 diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 471f835..8bd75e8 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -526,7 +526,7 @@ bool mpu6000_who_am_i() /* 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^8) // that means a CMP_FACTOR of 1024 (2^10) +#define GYRO_ACC_DIV_FACTOR (2^4) // that means a CMP_FACTOR of 1024 (2^10) #define GetMagnitude(x) (x*x) #define Low_Magnitude (GetMagnitude(0.85)) diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 25ba171..e296f2d 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -103,7 +103,7 @@ 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]*throttleRate; + 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; diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index e596fa9..38610a8 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -69,6 +69,8 @@ void systemTaskAccelerometer(void) //update the accelerometer data // uint8_t c = 97; // usart_transmit(&cliUsart, &c, 1, 1000000000); + + } void systemTaskAttitude(void) @@ -254,6 +256,10 @@ 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(); From 4986238d1e4b7ba9d76a4e6d312820adef9ce0af Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Fri, 18 Nov 2016 14:05:18 +0100 Subject: [PATCH 31/33] fast --- UAV-ControlSystem/src/Flight/pid.c | 1 + UAV-ControlSystem/src/drivers/accel_gyro.c | 12 +++++++++--- UAV-ControlSystem/src/drivers/motormix.c | 2 +- UAV-ControlSystem/src/tasks_main.c | 6 +++--- 4 files changed, 14 insertions(+), 7 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index fbb1f94..366a9a8 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -374,6 +374,7 @@ 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 { diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 8bd75e8..2a40d42 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -527,6 +527,7 @@ bool mpu6000_who_am_i() /* 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^4) // 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)) @@ -554,7 +555,7 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) 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] / 16; + smooth[i] = lpf_Acc[i] / ACCEL_LPF_FACTOR; //Save the sign(+/-) of the value sign[i] = (accelConv[i]< 0) ? -1 : 1; @@ -587,8 +588,13 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) 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; +// TODO: This is the original code. Test to set factors for them separatelly +// accel->rollAngle += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR; +// accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR; + + + accel->rollAngle += (a_RollAngle/4 - accel->rollAngle/GYRO_ACC_DIV_FACTOR); + accel->pitchAngle += (a_PitchAngle/4 - accel->pitchAngle/GYRO_ACC_DIV_FACTOR); } //Always constran the angular values within the possible ranges 90 to -90 diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index e296f2d..25ba171 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -103,7 +103,7 @@ 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]/**throttleRate*/; + 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; diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 38610a8..09e657d 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -256,9 +256,9 @@ 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); +// 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 From f6f6f50cdfad0299ae769f0ce438b1b7d4ced4cd Mon Sep 17 00:00:00 2001 From: philsson Date: Mon, 21 Nov 2016 09:03:08 +0100 Subject: [PATCH 32/33] Successful tests from friday. This code is not correct though. Regards ACC!!! Even though in baro2 branch --- UAV-ControlSystem/src/drivers/accel_gyro.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 2a40d42..ad788e8 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -581,7 +581,8 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) 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) + //if (Low_Magnitude < magnitude && magnitude < High_Magnitude) + if(0) { //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; @@ -593,8 +594,11 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) // accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR; - accel->rollAngle += (a_RollAngle/4 - accel->rollAngle/GYRO_ACC_DIV_FACTOR); - accel->pitchAngle += (a_PitchAngle/4 - accel->pitchAngle/GYRO_ACC_DIV_FACTOR); +// accel->rollAngle += (a_RollAngle/4 - accel->rollAngle/GYRO_ACC_DIV_FACTOR); +// accel->pitchAngle += (a_PitchAngle/4 - accel->pitchAngle/GYRO_ACC_DIV_FACTOR); + + accel->rollAngle = (a_RollAngle); + accel->pitchAngle = (a_PitchAngle); } //Always constran the angular values within the possible ranges 90 to -90 From 16b05ee20668ac9d0396f8ceb81e2f50e12264ec Mon Sep 17 00:00:00 2001 From: johan9107 Date: Mon, 21 Nov 2016 11:17:00 +0100 Subject: [PATCH 33/33] Improved angle update to accelerometer --- UAV-ControlSystem/inc/Flight/pid.h | 2 ++ UAV-ControlSystem/src/Flight/pid.c | 14 ++++++++------ UAV-ControlSystem/src/drivers/accel_gyro.c | 14 +++----------- UAV-ControlSystem/src/tasks_main.c | 14 +------------- 4 files changed, 14 insertions(+), 30 deletions(-) diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index ac014ac..fd6d62a 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -84,6 +84,8 @@ void pidInit(); **************************************************************************/ void pidRun(uint8_t ID); +void readAcc(void); + void pidEproom(void); #endif /* FLIGHT_PID_H_ */ diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 366a9a8..6e7a73c 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -31,7 +31,7 @@ #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 10 /*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 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)*/ @@ -69,7 +69,7 @@ float accRollFineTune = 0; float accPitchFineTune = 0; float throttleRate = 1; -int HoverForce = 1475;/*Struct profile for input data from sensor*/ +int HoverForce = 1475; /*Struct profile for input data from sensor*/ /************************************************************************** * BRIEF: Scales data from input range to output range * @@ -109,8 +109,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_ACCELEROMETER: - mpu6000_read_angle(&accelProfile,&gyroProfile); /*Reads data from accelerometer*/ - sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune; sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; @@ -245,7 +243,6 @@ 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]; @@ -341,7 +338,6 @@ void pidAccelerometer(void) PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; counterAcc +=1; } - } /************************************************************************** @@ -418,6 +414,12 @@ void pidRun(uint8_t ID) } } +void readAcc(void) +{ + /*Reads data from accelerometer*/ + mpu6000_read_angle(&accelProfile,&gyroProfile); +} + /*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/ /************************************************************************** diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 2a40d42..f0b87cc 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -526,7 +526,7 @@ bool mpu6000_who_am_i() /* 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^4) // that means a CMP_FACTOR of 1024 (2^10) +#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) @@ -588,16 +588,8 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) 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 -// TODO: This is the original code. Test to set factors for them separatelly -// accel->rollAngle += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR; -// accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR; - - - accel->rollAngle += (a_RollAngle/4 - accel->rollAngle/GYRO_ACC_DIV_FACTOR); - accel->pitchAngle += (a_PitchAngle/4 - accel->pitchAngle/GYRO_ACC_DIV_FACTOR); + accel->rollAngle += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR; + accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR; } - //Always constran the angular values within the possible ranges 90 to -90 - accel->rollAngle = constrainf(accel->rollAngle, -90,90); - accel->pitchAngle = constrainf(accel->pitchAngle, -90,90); } diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 09e657d..d5ab7b5 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -46,18 +46,10 @@ 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,12 +57,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)