From 46a52fc5ce276947618c6b103b030eebfb8f23a8 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 26 May 2012 12:05:04 -0500 Subject: [PATCH] Apply the virtual flybar stick scaling in stabilization.c because we need to know the true stick position to dampen the gyros appropriately. --- flight/Modules/ManualControl/manualcontrol.c | 6 +++--- flight/Modules/Stabilization/stabilization.c | 8 +++++++- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 73ba22f61..cc188e602 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -586,7 +586,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll * stabSettings.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : @@ -594,7 +594,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch * stabSettings.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : @@ -602,7 +602,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw * stabSettings.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 94b97536a..b08a90076 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -77,6 +77,7 @@ static xQueueHandle queue; float gyro_alpha = 0; float gyro_filtered[3] = {0,0,0}; float axis_lock_accum[3] = {0,0,0}; +float vbar_sensitivity[3] = {1, 1, 1}; uint8_t max_axis_lock = 0; uint8_t max_axislock_rate = 0; float weak_leveling_kp = 0; @@ -340,7 +341,7 @@ static void stabilizationTask(void* parameters) gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f); gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; } - float command = rateDesiredAxis[i] - gyro_gain * ( + float command = rateDesiredAxis[i] * vbar_sensitivity[i] - gyro_gain * ( vbar_integral[i] * pids[PID_VBAR_ROLL + i].i + gyro_filtered[i] * pids[PID_VBAR_ROLL + i].p); @@ -491,6 +492,11 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) pids[PID_VBAR_YAW].p = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KP]; pids[PID_VBAR_YAW].i = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KI]; + // Need to store the vbar sensitivity + vbar_sensitivity[0] = settings.VbarSensitivity[0]; + vbar_sensitivity[1] = settings.VbarSensitivity[1]; + vbar_sensitivity[2] = settings.VbarSensitivity[2]; + // Maximum deviation to accumulate for axis lock max_axis_lock = settings.MaxAxisLock; max_axislock_rate = settings.MaxAxisLockRate;