From ff5eca5fd7ad69ff8213611d9f8763cd9643a109 Mon Sep 17 00:00:00 2001 From: philsson Date: Tue, 29 Nov 2016 17:10:54 +0100 Subject: [PATCH] Implementation of inclination compensation on throttle --- UAV-ControlSystem/src/Flight/pid.c | 2 +- UAV-ControlSystem/src/drivers/motormix.c | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 125b7d2..5ae7201 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -113,7 +113,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; /*Checks the biggest angle */ - throttleRate = (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? 2 - cos(sensorValues[ROLL]*M_PI/180) : 2 - cos(sensorValues[PITCH]*M_PI/180); + throttleRate = cos(ABS_FLOAT(sensorValues[PITCH])*M_PI/180)*cos(ABS_FLOAT(sensorValues[ROLL])*M_PI/180); break; case PID_ID_COMPASS: diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index acfd7a6..6403d40 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -103,10 +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];//*throttleRate; + int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]; if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += HoverForce; + throttle = (throttle - 1000)/throttleRate + 1000; + /* Mixer Full Scale enabled */ if (flags_IsSet_ID(systemFlags_mixerfullscale_id)) {