From ce9d016e8117475b0670d3a84164a084440f1db9 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Thu, 20 Oct 2016 10:42:49 +0200 Subject: [PATCH 01/15] PID eeprom settings changed to correct index of PID profile PID eeprom settings changed to correct index of PID profile --- UAV-ControlSystem/src/Flight/pid.c | 4 ++++ UAV-ControlSystem/src/config/cli.c | 6 +++--- UAV-ControlSystem/src/main.c | 2 +- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index da126f9..b17a19e 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -419,6 +419,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) PidProfile[ID].PIDweight[PITCH] = 100; PidProfile[ID].PIDweight[YAW] = 100; + PidProfile[ID].P[ROLL] = 10; + PidProfile[ID].P[PITCH] = 10; + PidProfile[ID].P[YAW] = 10; + switch (ID) { case PID_ID_GYRO: diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 7f96636..3900980 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -626,15 +626,15 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { }, [COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF] = { - "pid_accel_pterm_yaw_lpf", COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 15, VAL_UINT_16, .valueRange = {0, 100} + "pid_accel_pterm_yaw_lpf", COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 100} }, [COMMAND_ID_PID_ACCEL_YAW_P_LIMIT] = { - "pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 17, VAL_UINT_16, .valueRange = {0, 100} + "pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 100} }, [COMMAND_ID_PID_ACCEL_OUT_LIMIT] = { - "pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 100} + "pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 100} }, }; diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index b69cd1d..9722f46 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -52,7 +52,7 @@ void init_system() readEEPROM(); //initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble - cliInit(USART6); + cliInit(USART3); //init sbus, using USART1 sbus_init(); From 7ca3bbec8ff9bbb216aee019ee927b6b5eb46c67 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 20 Oct 2016 18:22:40 +0200 Subject: [PATCH 02/15] Changed some min max values in CLI and changed direction of the GYRO --- UAV-ControlSystem/inc/drivers/accel_gyro.h | 2 +- UAV-ControlSystem/src/config/cli.c | 46 +++++++++++----------- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h index e8cd064..fd42421 100644 --- a/UAV-ControlSystem/inc/drivers/accel_gyro.h +++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h @@ -101,7 +101,7 @@ #define MPU6000_REV_D9 0x59 #define MPU6000_REV_D10 0x5A -#define YAW_ROT_0 +#define YAW_ROT_180 typedef struct gyro_t { int16_t gyroX, gyroY, gyroZ; /* Gyroscope data converted into °/s */ diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 3900980..22b3005 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -532,109 +532,109 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { //GYRO P [COMMAND_ID_PID_GYRO_P_ROLL] = { - "pid_gyro_p_roll", COMMAND_ID_PID_GYRO_P_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 100} + "pid_gyro_p_roll", COMMAND_ID_PID_GYRO_P_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 255} }, [COMMAND_ID_PID_GYRO_P_PITCH] = { - "pid_gyro_p_pitch", COMMAND_ID_PID_GYRO_P_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 6, VAL_UINT_8, .valueRange = {0, 100} + "pid_gyro_p_pitch", COMMAND_ID_PID_GYRO_P_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 6, VAL_UINT_8, .valueRange = {0, 255} }, [COMMAND_ID_PID_GYRO_P_YAW] = { - "pid_gyro_p_yaw", COMMAND_ID_PID_GYRO_P_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 7, VAL_UINT_8, .valueRange = {0, 100} + "pid_gyro_p_yaw", COMMAND_ID_PID_GYRO_P_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 7, VAL_UINT_8, .valueRange = {0, 255} }, //GYRO I [COMMAND_ID_PID_GYRO_I_ROLL] = { - "pid_gyro_i_roll", COMMAND_ID_PID_GYRO_I_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 100} + "pid_gyro_i_roll", COMMAND_ID_PID_GYRO_I_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 255} }, [COMMAND_ID_PID_GYRO_I_PITCH] = { - "pid_gyro_i_pitch", COMMAND_ID_PID_GYRO_I_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 9, VAL_UINT_8, .valueRange = {0, 100} + "pid_gyro_i_pitch", COMMAND_ID_PID_GYRO_I_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 9, VAL_UINT_8, .valueRange = {0, 255} }, [COMMAND_ID_PID_GYRO_I_YAW] = { - "pid_gyro_i_yaw", COMMAND_ID_PID_GYRO_I_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 10, VAL_UINT_8, .valueRange = {0, 100} + "pid_gyro_i_yaw", COMMAND_ID_PID_GYRO_I_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 10, VAL_UINT_8, .valueRange = {0, 255} }, //GYRO D [COMMAND_ID_PID_GYRO_D_ROLL] = { - "pid_gyro_d_roll", COMMAND_ID_PID_GYRO_D_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 100} + "pid_gyro_d_roll", COMMAND_ID_PID_GYRO_D_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 255} }, [COMMAND_ID_PID_GYRO_D_PITCH] = { - "pid_gyro_d_pitch", COMMAND_ID_PID_GYRO_D_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 12, VAL_UINT_8, .valueRange = {0, 100} + "pid_gyro_d_pitch", COMMAND_ID_PID_GYRO_D_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 12, VAL_UINT_8, .valueRange = {0, 255} }, [COMMAND_ID_PID_GYRO_D_YAW] = { - "pid_gyro_d_yaw", COMMAND_ID_PID_GYRO_D_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 13, VAL_UINT_8, .valueRange = {0, 100} + "pid_gyro_d_yaw", COMMAND_ID_PID_GYRO_D_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 13, VAL_UINT_8, .valueRange = {0, 255} }, //Gyro filter and limits [COMMAND_ID_PID_GYRO_DTERM_LPF] = { - "pid_gyro_dterm_lpf", COMMAND_ID_PID_GYRO_DTERM_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 100} + "pid_gyro_dterm_lpf", COMMAND_ID_PID_GYRO_DTERM_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 255} }, [COMMAND_ID_PID_GYRO_PTERM_YAW_LPF] = { - "pid_gyro_pterm_yaw_lpf", COMMAND_ID_PID_GYRO_PTERM_YAW_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 100} + "pid_gyro_pterm_yaw_lpf", COMMAND_ID_PID_GYRO_PTERM_YAW_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 255} }, [COMMAND_ID_PID_GYRO_YAW_P_LIMIT] = { - "pid_gyro_yaw_lpf", COMMAND_ID_PID_GYRO_YAW_P_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 100} + "pid_gyro_yaw_lpf", COMMAND_ID_PID_GYRO_YAW_P_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 255} }, [COMMAND_ID_PID_GYRO_OUT_LIMIT] = { - "pid_gyro_out_limit", COMMAND_ID_PID_GYRO_OUT_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 100} + "pid_gyro_out_limit", COMMAND_ID_PID_GYRO_OUT_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 255} }, //ACCEL P [COMMAND_ID_PID_ACCEL_P_ROLL] = { - "pid_accel_p_roll", COMMAND_ID_PID_ACCEL_P_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 100} + "pid_accel_p_roll", COMMAND_ID_PID_ACCEL_P_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 255} }, [COMMAND_ID_PID_ACCEL_P_PITCH] = { - "pid_accel_p_pitch", COMMAND_ID_PID_ACCEL_P_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 6, VAL_UINT_8, .valueRange = {0, 100} + "pid_accel_p_pitch", COMMAND_ID_PID_ACCEL_P_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 6, VAL_UINT_8, .valueRange = {0, 255} }, //ACCEL I [COMMAND_ID_PID_ACCEL_I_ROLL] = { - "pid_accel_i_roll", COMMAND_ID_PID_ACCEL_I_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 100} + "pid_accel_i_roll", COMMAND_ID_PID_ACCEL_I_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 255} }, [COMMAND_ID_PID_ACCEL_I_PITCH] = { - "pid_accel_i_pitch", COMMAND_ID_PID_ACCEL_I_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 9, VAL_UINT_8, .valueRange = {0, 100} + "pid_accel_i_pitch", COMMAND_ID_PID_ACCEL_I_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 9, VAL_UINT_8, .valueRange = {0, 255} }, //ACCEL D [COMMAND_ID_PID_ACCEL_D_ROLL] = { - "pid_accel_d_roll", COMMAND_ID_PID_ACCEL_D_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 100} + "pid_accel_d_roll", COMMAND_ID_PID_ACCEL_D_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 255} }, [COMMAND_ID_PID_ACCEL_D_PITCH] = { - "pid_accel_d_pitch", COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 12, VAL_UINT_8, .valueRange = {0, 100} + "pid_accel_d_pitch", COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 12, VAL_UINT_8, .valueRange = {0, 255} }, //ACCEL Filters and limits [COMMAND_ID_PID_ACCEL_DTERM_LPF] = { - "pid_accel_dterm_lpf", COMMAND_ID_PID_ACCEL_DTERM_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 100} + "pid_accel_dterm_lpf", COMMAND_ID_PID_ACCEL_DTERM_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 65000} }, [COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF] = { - "pid_accel_pterm_yaw_lpf", COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 100} + "pid_accel_pterm_yaw_lpf", COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 65000} }, [COMMAND_ID_PID_ACCEL_YAW_P_LIMIT] = { - "pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 100} + "pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 65000} }, [COMMAND_ID_PID_ACCEL_OUT_LIMIT] = { - "pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 100} + "pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000} }, }; From 1f6bf53f62b5a18dfbf0ecc1bc3cf990db610aa4 Mon Sep 17 00:00:00 2001 From: philsson Date: Mon, 24 Oct 2016 11:55:52 +0200 Subject: [PATCH 03/15] Change in calibration. MinCommand set to 990 to be lower than lowest calibration of 1000. Max calibration is set to MAX_Pulse which is 1950 at this time instead of 2000 which we calibrated with earlier --- UAV-ControlSystem/src/drivers/motormix.c | 2 +- UAV-ControlSystem/src/drivers/motors.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 4179c9a..3bf211f 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -47,7 +47,7 @@ uint16_t motor_output[MOTOR_COUNT]; mixerConfig_s mixerConfig = { .minThrottle = 1050, .maxThrottle = MAX_PULSE - 100, - .minCommand = 1000, + .minCommand = 990, .maxCommand = MAX_PULSE, .minCheck = 1010, .pid_at_min_throttle = true, diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c index b2a83d9..53bb569 100644 --- a/UAV-ControlSystem/src/drivers/motors.c +++ b/UAV-ControlSystem/src/drivers/motors.c @@ -248,7 +248,7 @@ void calibrateMotors() pwmActivateAllMotors(); //First set the motor output to the maximum allowed throttle - for (uint8_t i = 1; i < 11; i++ ) pwmAdjustSpeedOfMotor(i,MotorPWMPeriode); + for (uint8_t i = 1; i < 11; i++ ) pwmAdjustSpeedOfMotor(i,MAX_PULSE); //wait for a little while HAL_Delay(6200); From c0cb3ffe17a2247805ab09a8cf1b0c8e3a806c2a Mon Sep 17 00:00:00 2001 From: johan9107 Date: Mon, 24 Oct 2016 14:37:01 +0200 Subject: [PATCH 04/15] PID, added correct calculations to accelerometer data PID, added correct calculations to accelerometer data --- UAV-ControlSystem/src/Flight/pid.c | 39 ++++++++++++++++++---- UAV-ControlSystem/src/drivers/accel_gyro.c | 2 ++ 2 files changed, 34 insertions(+), 7 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index b17a19e..7814df5 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -25,7 +25,7 @@ #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/ -#define ACCELEROMETER_RANGE 90 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ +#define ACCELEROMETER_RANGE 20 /*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)*/ @@ -56,6 +56,31 @@ accel_t accelProfile; /*Struct profile for input data from sensor*/ gyro_t gyroProfile; /*Struct profile for input data from sensor*/ +/************************************************************************** +* 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(z_axis, sqrt(x_axis*x_axis + y_axis*y_axis))*180/M_PI - 90; + angle = (x_axis < 0) ? -angle : angle; /*CW (right, form behind) = pos angle*/ + break; + case PITCH: + angle = atan2(sqrt(x_axis*x_axis + z_axis*z_axis), y_axis)*180/M_PI - 90; /*down (the front down against ground) = pos angle*/ + break; + default: + angle = 0; + break; + } + + return angle; +} + /************************************************************************** * BRIEF: Scales data from input range to output range * * INFORMATION: * @@ -83,7 +108,7 @@ float constrainf(float amt, int low, int high) * BRIEF: Update current sensor values * * INFORMATION: * **************************************************************************/ -void getCurrentValues(float *sensorValues, uint8_t ID_profile) +void getCurrentValues(float sensorValues[3], uint8_t ID_profile) { switch (ID_profile) { @@ -101,8 +126,8 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile) mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ /*May need Low pass filter since the accelerometer may drift*/ - sensorValues[ROLL] = atan2(accelProfile.accelXconv,accelProfile.accelZconv)*180*M_PI; - sensorValues[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI; + sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); break; case PID_ID_COMPASS: @@ -434,9 +459,9 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) break; case PID_ID_ACCELEROMETER: - PidProfile[ID].pidEnabled = false; - //PidProfile[ID].dterm_lpf = 90; - //PidProfile[ID].pid_out_limit = 3000; + PidProfile[ID].pidEnabled = true; + PidProfile[ID].dterm_lpf = 90; + PidProfile[ID].pid_out_limit = 1000; break; case PID_ID_COMPASS: diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 95ae8d0..45851c8 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -180,6 +180,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel) if(!mpu6000_transmit(reg, 2)) return false; + HAL_Delay(60); + mpu6000_read_offset(gyro, accel); return true; From 1ef8f56ed63661043ca135bf83e40eaac84471d8 Mon Sep 17 00:00:00 2001 From: philsson Date: Mon, 24 Oct 2016 17:34:44 +0200 Subject: [PATCH 05/15] Changes to acc functionality. OBS! Not working correctly --- UAV-ControlSystem/src/Flight/pid.c | 8 +++++--- UAV-ControlSystem/src/config/cli.c | 18 +++++++++++++++++- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 7814df5..082ef17 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -18,6 +18,7 @@ #include "drivers/sbus.h" #include "scheduler/scheduler.h" #include +#include "drivers/failsafe_toggles.h" #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ #define ITERM_SCALE 0.244381f /*I-term used as a scale value to the PID controller*/ @@ -87,7 +88,8 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons **************************************************************************/ float convertData(int inputRange, int outputRange, int offset, float value) { - return (outputRange/inputRange)*(value-offset); + return ((float)outputRange/(float)inputRange)*(value-(float)offset); + //return 1.0; } /************************************************************************** @@ -154,7 +156,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) { case PID_ID_GYRO: - if (!PidProfile[PID_ID_ACCELEROMETER].pidEnabled) + if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id))) { desiredCommand[ROLL] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL]); desiredCommand[PITCH] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH]); @@ -326,7 +328,7 @@ void pidRun(uint8_t ID) break; case PID_ID_ACCELEROMETER: - if (!PidProfile[PID_ID_ACCELEROMETER].pidEnabled) + if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id))) { PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll; PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 22b3005..acc5305 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -266,6 +266,10 @@ typedef enum COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, COMMAND_ID_PID_ACCEL_OUT_LIMIT, + /* Enable the different pid loops */ + COMMAND_ID_PID_GYRO_ISENABLED, + COMMAND_ID_PID_ACCEL_ISENABLED, + /* Counter for the amount of commands */ COMMAND_ID_COUNT, @@ -634,8 +638,20 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { }, [COMMAND_ID_PID_ACCEL_OUT_LIMIT] = { - "pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000} + "pid_accel_out_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000} }, + + /* Enable pid loops */ + [COMMAND_ID_PID_GYRO_ISENABLED] = + { + "pid_gyro_isenabled", COMMAND_ID_PID_GYRO_ISENABLED, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1} + }, + [COMMAND_ID_PID_ACCEL_ISENABLED] = + { + "pid_accel_isenabled", COMMAND_ID_PID_ACCEL_ISENABLED, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1} + }, + + }; /*********************************************************************** From 4da4736257343b0e882886cad7300ebb09ed3faf Mon Sep 17 00:00:00 2001 From: philsson Date: Tue, 25 Oct 2016 09:17:45 +0200 Subject: [PATCH 06/15] Fix for motormix low. Not tested! --- UAV-ControlSystem/src/drivers/motormix.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 3bf211f..ea8dd04 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -199,6 +199,10 @@ void mix() PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); } + for (int i = 0; i < MOTOR_COUNT; i++) + // Find the minimum and maximum motor output + if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i]; + /* Mixer Low Scale - Scaling output around low throttle */ if (flags_IsSet_ID(systemFlags_mixerlowscale_id)) @@ -219,6 +223,9 @@ void mix() } } + /* Recalculating RPY_Mix_Min */ + RPY_Mix_Min = throttleMid; + for (int i = 0; i < MOTOR_COUNT; i++) { // Find the minimum and maximum motor output From 75dadc7484716a91c8515ca9bd817a5aa4f68832 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Tue, 25 Oct 2016 09:43:38 +0200 Subject: [PATCH 07/15] Chnaged port from usart 3 to 6 on cli --- UAV-ControlSystem/src/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 9722f46..b69cd1d 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -52,7 +52,7 @@ void init_system() readEEPROM(); //initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble - cliInit(USART3); + cliInit(USART6); //init sbus, using USART1 sbus_init(); From 6bdcb6e853e17adf5c6c6458f0246a6b58a1b4b6 Mon Sep 17 00:00:00 2001 From: philsson Date: Tue, 25 Oct 2016 09:50:23 +0200 Subject: [PATCH 08/15] Some more fixes on mixer. Not working well! --- UAV-ControlSystem/src/drivers/motormix.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index ea8dd04..22624cf 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -199,14 +199,16 @@ void mix() PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); } - for (int i = 0; i < MOTOR_COUNT; i++) - // Find the minimum and maximum motor output - if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i]; + /* Mixer Low Scale - Scaling output around low throttle */ if (flags_IsSet_ID(systemFlags_mixerlowscale_id)) { + for (int i = 0; i < MOTOR_COUNT; i++) + // Find the minimum and maximum motor output + if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i]; + uint16_t RPY_Mix_Min_Overshoot = (RPY_Mix_Min < mixerConfig.minThrottle) ? mixerConfig.minThrottle - RPY_Mix_Min : 0; if (RPY_Mix_Min_Overshoot > 0 ) //|| RPY_Mix_Max_Overshoot > 0) @@ -221,10 +223,11 @@ void mix() for (int i = 0; i < MOTOR_COUNT; i++) RPY_Mix[i] = (((float)(RPY_Mix[i] - throttle)) * (1 - mix_scale_reduction)) + throttle; } + /* Recalculating RPY_Mix_Min */ + RPY_Mix_Min = throttleMid; } - /* Recalculating RPY_Mix_Min */ - RPY_Mix_Min = throttleMid; + for (int i = 0; i < MOTOR_COUNT; i++) { From facfcb7023c5c1adfc461a4e55ddae26bfb2f168 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Tue, 25 Oct 2016 10:13:49 +0200 Subject: [PATCH 09/15] PID accelerometer added low pass filter --- UAV-ControlSystem/src/Flight/pid.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 082ef17..2ddce64 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -56,6 +56,7 @@ pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0}; accel_t accelProfile; /*Struct profile for input data from sensor*/ gyro_t gyroProfile; /*Struct profile for input data from sensor*/ +pt1Filter_t accelFilter[2] = {0}; /************************************************************************** * BRIEF: Calculates angle from accelerometer * @@ -127,10 +128,19 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ + + /*May need Low pass filter since the accelerometer may drift*/ + 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: From 76fb5c8128994ce9812a10e4f79e7b237d511453 Mon Sep 17 00:00:00 2001 From: philsson Date: Tue, 25 Oct 2016 10:13:21 +0200 Subject: [PATCH 10/15] Motormixer fixed now! The mid mixer does not max out motors anymore (probably not) --- UAV-ControlSystem/inc/utilities.h | 2 +- UAV-ControlSystem/src/utilities.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h index 26fdf58..446ddea 100644 --- a/UAV-ControlSystem/inc/utilities.h +++ b/UAV-ControlSystem/inc/utilities.h @@ -83,6 +83,6 @@ void Error_Handler(void); uint8_t reverse(uint8_t byte); -uint16_t constrain(uint16_t value, uint16_t min, uint16_t max); +int16_t constrain(int16_t value, int16_t min, int16_t max); #endif /* UTILITIES_H_ */ diff --git a/UAV-ControlSystem/src/utilities.c b/UAV-ControlSystem/src/utilities.c index 2e22185..7c2192c 100644 --- a/UAV-ControlSystem/src/utilities.c +++ b/UAV-ControlSystem/src/utilities.c @@ -219,7 +219,7 @@ uint8_t reverse(uint8_t byte) return byte; } -uint16_t constrain(uint16_t value, uint16_t min, uint16_t max) +int16_t constrain(int16_t value, int16_t min, int16_t max) { if (value < min) return min; From 69c9f22dadedf6eeebb7a53bf21589969cd6e495 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 26 Oct 2016 16:22:33 +0200 Subject: [PATCH 11/15] Pid update angle caluclations --- UAV-ControlSystem/src/Flight/pid.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 2ddce64..065ad46 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -65,15 +65,21 @@ pt1Filter_t accelFilter[2] = {0}; float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis) { float angle; + float angle_offset = (z_axis < 0 )? 90: 0; switch (axis) { case ROLL: - angle = atan2(z_axis, sqrt(x_axis*x_axis + y_axis*y_axis))*180/M_PI - 90; - angle = (x_axis < 0) ? -angle : angle; /*CW (right, form behind) = pos angle*/ + + 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(sqrt(x_axis*x_axis + z_axis*z_axis), y_axis)*180/M_PI - 90; /*down (the front down against ground) = pos angle*/ + + 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; From c3e969e982ed81e95d4aa710aaf71ea5219856c4 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Thu, 27 Oct 2016 11:55:16 +0200 Subject: [PATCH 12/15] PID Added new values to improve filter --- UAV-ControlSystem/src/Flight/pid.c | 20 +++++++++++++++++--- UAV-ControlSystem/src/drivers/motormix.c | 8 ++++---- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 065ad46..98ffe07 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -26,7 +26,7 @@ #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/ -#define ACCELEROMETER_RANGE 20 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ +#define ACCELEROMETER_RANGE 35 /*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)*/ @@ -113,6 +113,8 @@ float constrainf(float amt, int low, int high) return amt; } + +float oldSensorValue[2] = {0}; /************************************************************************** * BRIEF: Update current sensor values * * INFORMATION: * @@ -136,10 +138,20 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) + float alpha = 0.5; /*May need Low pass filter since the accelerometer may drift*/ - sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - sensorValues[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); + + sensorValues[ROLL] = alpha*X_roll + (1-alpha)*oldSensorValue[0]; + sensorValues[PITCH] = alpha*X_pitch + (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); @@ -384,6 +396,8 @@ void pidRun(uint8_t ID) } } +/*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/ + /************************************************************************** * BRIEF: Initializes a certain pidbuffer profile connected to * * a PID controller * diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 22624cf..3da3fec 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -114,8 +114,8 @@ void mix() * Calculation is: Output from control system * weight from model for each motor */ RPY_Mix[i] = \ - PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \ - PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \ + (int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \ + (int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \ PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); @@ -194,8 +194,8 @@ void mix() { RPY_Mix[i] = \ throttle * mixerUAV[i].throttle + \ - PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \ - PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \ + (int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \ + (int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \ PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); } From 3d3d16ceadd4dc842c9e9abb3d6d55768971c0c5 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Thu, 27 Oct 2016 14:30:26 +0200 Subject: [PATCH 13/15] PID added new lowpas filter and average calculations to Accelerometer values --- UAV-ControlSystem/src/Flight/pid.c | 30 ++++++++++++++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 98ffe07..d263d18 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -115,12 +115,19 @@ float constrainf(float amt, int low, int high) float oldSensorValue[2] = {0}; +float oldSensorValueRoll[50] = {0}; +float oldSensorValuePitch[50] = {0}; + +int i = 0; + /************************************************************************** * BRIEF: Update current sensor values * * INFORMATION: * **************************************************************************/ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) { + + switch (ID_profile) { case PID_ID_GYRO: @@ -144,9 +151,28 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - sensorValues[ROLL] = alpha*X_roll + (1-alpha)*oldSensorValue[0]; - sensorValues[PITCH] = alpha*X_pitch + (1-alpha)*oldSensorValue[1]; + oldSensorValueRoll[i] = X_roll; + oldSensorValuePitch[i] = X_pitch; + float RollValue = 0; + float PitchValue = 0; + + for (int ii = 0; ii < 50; ii++) + { + RollValue = RollValue + oldSensorValueRoll[ii]; + PitchValue = PitchValue + oldSensorValuePitch[ii]; + + } + + + i = (i < 49)? i + 1:0; + + sensorValues[ROLL] = RollValue/50; + sensorValues[PITCH] = PitchValue/50; + + sensorValues[ROLL] = alpha*RollValue/50 + (1-alpha)*oldSensorValue[0]; + sensorValues[PITCH] = alpha*PitchValue/50 + (1-alpha)*oldSensorValue[1]; +// oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[1] = sensorValues[PITCH]; From 428a6140b412af0c761f76fb22cb524569fcf424 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Fri, 28 Oct 2016 08:39:05 +0200 Subject: [PATCH 14/15] PID added new dweight values --- UAV-ControlSystem/src/Flight/pid.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index d263d18..335013a 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -265,7 +265,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, /* -----calculate P component ---- */ - float PTerm = PTERM_SCALE * rateError * pidProfile->P[axis] * pidProfile-> PIDweight[axis] / 100; + float PTerm = PTERM_SCALE * rateError * (float)pidProfile->P[axis] * (float)pidProfile-> PIDweight[axis] / 100.0; // Constrain YAW by yaw_p_limit value if (axis == YAW) @@ -328,7 +328,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, delta = pt1FilterApply4(&pidProfileBuff->deltaFilter[axis], delta, pidProfile->dterm_lpf, pidProfileBuff->dT); } - DTerm = DTERM_SCALE * delta * pidProfile->D[axis] * pidProfile->PIDweight[axis] / 100; + DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0; DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D); } @@ -520,6 +520,8 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) PidProfile[ID].pidEnabled = true; PidProfile[ID].dterm_lpf = 90; PidProfile[ID].pid_out_limit = 1000; + pidProfile[ID].PIDweight[ROLL] = 50; + pidProfile[ID].PIDweight[PITCH] = 50; break; case PID_ID_COMPASS: From c57ec513973d7f444925d62f8ac3cd9bb4295c63 Mon Sep 17 00:00:00 2001 From: philsson Date: Fri, 28 Oct 2016 10:22:42 +0200 Subject: [PATCH 15/15] Temp fix for eeprom problem --- UAV-ControlSystem/inc/Flight/pid.h | 2 ++ UAV-ControlSystem/src/Flight/pid.c | 30 +++++++++++++++++++++++++----- UAV-ControlSystem/src/main.c | 4 +++- 3 files changed, 30 insertions(+), 6 deletions(-) diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index dd9f80f..a19bd81 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -75,4 +75,6 @@ void pidInit(); **************************************************************************/ void pidRun(uint8_t ID); +void pidEproom(void); + #endif /* FLIGHT_PID_H_ */ diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 335013a..b614907 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -498,9 +498,6 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) PidProfile[ID].PID_Out[PITCH] = 0; PidProfile[ID].PID_Out[YAW] = 0; - PidProfile[ID].PIDweight[ROLL] = 100; - PidProfile[ID].PIDweight[PITCH] = 100; - PidProfile[ID].PIDweight[YAW] = 100; PidProfile[ID].P[ROLL] = 10; PidProfile[ID].P[PITCH] = 10; @@ -510,6 +507,10 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) { case PID_ID_GYRO: + PidProfile[ID].PIDweight[ROLL] = 100; + PidProfile[ID].PIDweight[PITCH] = 100; + PidProfile[ID].PIDweight[YAW] = 100; + PidProfile[ID].pidEnabled = true; PidProfile[ID].dterm_lpf = 90; PidProfile[ID].pid_out_limit = 3000; @@ -517,20 +518,30 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) break; case PID_ID_ACCELEROMETER: + PidProfile[ID].PIDweight[ROLL] = 2; + PidProfile[ID].PIDweight[PITCH] = 2; + PidProfile[ID].PIDweight[YAW] = 100; + PidProfile[ID].pidEnabled = true; PidProfile[ID].dterm_lpf = 90; PidProfile[ID].pid_out_limit = 1000; - pidProfile[ID].PIDweight[ROLL] = 50; - pidProfile[ID].PIDweight[PITCH] = 50; break; case PID_ID_COMPASS: + 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].PIDweight[ROLL] = 100; + PidProfile[ID].PIDweight[PITCH] = 100; + PidProfile[ID].PIDweight[YAW] = 100; + PidProfile[ID].pidEnabled = false; break; @@ -560,3 +571,12 @@ void pidInit() pidUAVInit(&PidProfile[PID_ID_BAROMETER], PID_ID_BAROMETER); pidUAVInit(&PidProfile[PID_ID_COMPASS], PID_ID_COMPASS); } + +void pidEproom(void) +{ + + PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2; + PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2; + PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100; + +} diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index b69cd1d..3edd170 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -51,8 +51,10 @@ void init_system() /* read saved variables from eeprom, in most cases eeprom should be read after a lot of the initializes */ readEEPROM(); + pidEproom(); + //initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble - cliInit(USART6); + cliInit(USART3); //init sbus, using USART1 sbus_init();