From aa93a954d89b4f7fd73eb6ffd2bd4611322ed13b Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 10 Nov 2016 13:30:24 +0100 Subject: [PATCH] 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))