From f20adf00f5f377b32213ed10e563819715fd82a8 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Thu, 14 Jan 2016 22:55:12 +0100 Subject: [PATCH 1/2] LP-214 Add Fpv Camera tilt compensation --- .../modules/ManualControl/stabilizedhandler.c | 23 ++ .../gcs/src/plugins/config/stabilization.ui | 230 ++++++++++++++---- .../uavobjectdefinition/stabilizationbank.xml | 2 + .../stabilizationsettingsbank1.xml | 2 + .../stabilizationsettingsbank2.xml | 2 + .../stabilizationsettingsbank3.xml | 2 + 6 files changed, 214 insertions(+), 47 deletions(-) diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index e2361b1f9..43f9aa155 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -30,6 +30,7 @@ #include "inc/manualcontrol.h" #include +#include #include #include #include @@ -42,6 +43,10 @@ // Private functions static float applyExpo(float value, float expo); +// Private variables +static uint8_t currentFpvTiltAngle = 0; +static float cosAngle = 0.0f; +static float sinAngle = 0.0f; static float applyExpo(float value, float expo) { @@ -90,6 +95,24 @@ void stabilizedHandler(bool newinit) cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll); cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch); cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw); + + if (stabSettings.FpvCamTiltCompensation > 0) { + // Reduce Cpu load + if (currentFpvTiltAngle != stabSettings.FpvCamTiltCompensation) { + cosAngle = cos_lookup_deg((float)stabSettings.FpvCamTiltCompensation); + sinAngle = sin_lookup_deg((float)stabSettings.FpvCamTiltCompensation); + currentFpvTiltAngle = stabSettings.FpvCamTiltCompensation; + } + float rollCommand = cmd.Roll; + float yawCommand = cmd.Yaw; + + // http://shrediquette.blogspot.de/2016/01/some-thoughts-on-camera-tilt.html + // Roll_output = cos(camera_tilt) * Roll_input - sin(camera_tilt) * Yaw_input + // Yaw_output = sin(camera_tilt) * Roll_input + cos(camera_tilt) * Yaw_input + cmd.Roll = boundf((cosAngle * rollCommand) - (sinAngle * yawCommand), -1.0f, 1.0f); + cmd.Yaw = boundf((cosAngle * yawCommand) + (sinAngle * rollCommand), -1.0f, 1.0f); + } + uint8_t *stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); diff --git a/ground/gcs/src/plugins/config/stabilization.ui b/ground/gcs/src/plugins/config/stabilization.ui index 0c25b1449..8ae4efc37 100644 --- a/ground/gcs/src/plugins/config/stabilization.ui +++ b/ground/gcs/src/plugins/config/stabilization.ui @@ -16898,53 +16898,182 @@ border-radius: 5; - - - Pirouette Compensation + + + 20 - - - - - - 0 - 27 - + + + + Pirouette Compensation + + + + + + + 0 + 27 + + + + Enable pirouette compensation + + + + objname:StabilizationSettingsBankX + fieldname:EnablePiroComp + buttongroup:55 + + + + + + + + Reset all values to GCS defaults + + + + + + Default + + + + objname:StabilizationSettings + button:default + buttongroup:55 + + + + + + + + + + + FPV Camera Tilt Compensation + + + + 9 - - Enable pirouette compensation + + 9 - - - objname:StabilizationSettingsBankX - fieldname:EnablePiroComp - buttongroup:55 - + + 9 - - - - - - Reset all values to GCS defaults + + 9 - - - - - Default - - - - objname:StabilizationSettings - button:default - buttongroup:55 - - - - - - + + + + Reset value to GCS defaults + + + + + + Default + + + + objname:StabilizationSettings + button:default + buttongroup:56 + + + + + + + + Camera Tilt Angle (deg) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 80 + 22 + + + + + 80 + 22 + + + + Qt::StrongFocus + + + Camera tilt angle from 0 to 50 degrees (0 to disable compensation). + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + 0 + + + 0.000000000000000 + + + 50.000000000000000 + + + 0.000000000000000 + + + + objname:StabilizationSettingsBankX + fieldname:FpvCamTiltCompensation + haslimits:no + scale:1 + buttongroup:56 + + + + + + + + Qt::Horizontal + + + QSizePolicy::Minimum + + + + 40 + 20 + + + + + + + + @@ -26113,19 +26242,18 @@ Useful if you have accidentally changed some settings. - tabWidget - scrollArea basicResponsivenessCheckBox - pushButton - AttitudeResponsivenessSlider - spinBox RateResponsivenessSlider + spinBox spinBox_2 RateYawResponsivenessSlider spinBox_5 pushButton_21 + pushButton + FpvCamTiltCompensation AcroFactorRollSlider AcroFactorRollSpinBox + pushButton_14 AcroFactorPitchSlider AcroFactorPitchSpinBox pushButton_12 @@ -26234,6 +26362,14 @@ Useful if you have accidentally changed some settings. stabilizationReloadBoardData_6 saveStabilizationToRAM_6 saveStabilizationToSD_6 + checkBoxLinkAcroFactors + tabWidget + AttitudeResponsivenessSlider + scrollArea + VelKdSlider + VelKd + VelBetaSlider + VelBeta diff --git a/shared/uavobjectdefinition/stabilizationbank.xml b/shared/uavobjectdefinition/stabilizationbank.xml index f4d5eaa85..168cc2644 100644 --- a/shared/uavobjectdefinition/stabilizationbank.xml +++ b/shared/uavobjectdefinition/stabilizationbank.xml @@ -20,6 +20,8 @@ + + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml index c1aca4f67..a7bd0c105 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml @@ -20,6 +20,8 @@ + + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml index c4e8f7617..fdf0ad5c3 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml @@ -20,6 +20,8 @@ + + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml index 69d6f364d..056b9ef30 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml @@ -20,6 +20,8 @@ + + From 580ef50efb5f15eae4df41a97eb570518516f945 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 16 Jan 2016 11:39:00 +0100 Subject: [PATCH 2/2] LP-214 Calc changes --- flight/modules/ManualControl/stabilizedhandler.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 43f9aa155..8cb6d3156 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -107,10 +107,10 @@ void stabilizedHandler(bool newinit) float yawCommand = cmd.Yaw; // http://shrediquette.blogspot.de/2016/01/some-thoughts-on-camera-tilt.html - // Roll_output = cos(camera_tilt) * Roll_input - sin(camera_tilt) * Yaw_input - // Yaw_output = sin(camera_tilt) * Roll_input + cos(camera_tilt) * Yaw_input - cmd.Roll = boundf((cosAngle * rollCommand) - (sinAngle * yawCommand), -1.0f, 1.0f); - cmd.Yaw = boundf((cosAngle * yawCommand) + (sinAngle * rollCommand), -1.0f, 1.0f); + // When Roll right, add negative Yaw. + // When Yaw left, add negative Roll. + cmd.Roll = boundf((cosAngle * rollCommand) + (sinAngle * yawCommand), -1.0f, 1.0f); + cmd.Yaw = boundf((cosAngle * yawCommand) - (sinAngle * rollCommand), -1.0f, 1.0f); } uint8_t *stab_settings;