mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-30 15:52:12 +01:00
LP-214 Add Fpv Camera tilt compensation
This commit is contained in:
Laurent Lalanne
Philippe Renon
parent
27afc79767
commit
a46bf079de
@ -30,6 +30,7 @@
|
|||||||
|
|
||||||
#include "inc/manualcontrol.h"
|
#include "inc/manualcontrol.h"
|
||||||
#include <mathmisc.h>
|
#include <mathmisc.h>
|
||||||
|
#include <sin_lookup.h>
|
||||||
#include <manualcontrolcommand.h>
|
#include <manualcontrolcommand.h>
|
||||||
#include <stabilizationdesired.h>
|
#include <stabilizationdesired.h>
|
||||||
#include <flightmodesettings.h>
|
#include <flightmodesettings.h>
|
||||||
@ -42,6 +43,10 @@
|
|||||||
// Private functions
|
// Private functions
|
||||||
static float applyExpo(float value, float expo);
|
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)
|
static float applyExpo(float value, float expo)
|
||||||
{
|
{
|
||||||
@ -90,6 +95,24 @@ void stabilizedHandler(bool newinit)
|
|||||||
cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
|
cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
|
||||||
cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
|
cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
|
||||||
cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
|
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;
|
uint8_t *stab_settings;
|
||||||
FlightStatusData flightStatus;
|
FlightStatusData flightStatus;
|
||||||
FlightStatusGet(&flightStatus);
|
FlightStatusGet(&flightStatus);
|
||||||
|
|||||||
@ -16898,53 +16898,182 @@ border-radius: 5;</string>
|
|||||||
</layout>
|
</layout>
|
||||||
</item>
|
</item>
|
||||||
<item>
|
<item>
|
||||||
<widget class="QGroupBox" name="groupBox_10">
|
<layout class="QHBoxLayout" name="horizontalLayout_7">
|
||||||
<property name="title">
|
<property name="bottomMargin">
|
||||||
<string>Pirouette Compensation</string>
|
<number>20</number>
|
||||||
</property>
|
</property>
|
||||||
<layout class="QGridLayout" name="gridLayout_32">
|
<item>
|
||||||
<item row="1" column="0">
|
<widget class="QGroupBox" name="groupBox_10">
|
||||||
<widget class="QCheckBox" name="enableThrustPIDScalingCheckBox_2">
|
<property name="title">
|
||||||
<property name="minimumSize">
|
<string>Pirouette Compensation</string>
|
||||||
<size>
|
</property>
|
||||||
<width>0</width>
|
<layout class="QGridLayout" name="gridLayout_32">
|
||||||
<height>27</height>
|
<item row="1" column="0">
|
||||||
</size>
|
<widget class="QCheckBox" name="enableThrustPIDScalingCheckBox_2">
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>0</width>
|
||||||
|
<height>27</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string>Enable pirouette compensation</string>
|
||||||
|
</property>
|
||||||
|
<property name="objrelation" stdset="0">
|
||||||
|
<stringlist>
|
||||||
|
<string>objname:StabilizationSettingsBankX</string>
|
||||||
|
<string>fieldname:EnablePiroComp</string>
|
||||||
|
<string>buttongroup:55</string>
|
||||||
|
</stringlist>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="0" column="0" alignment="Qt::AlignRight">
|
||||||
|
<widget class="QPushButton" name="pushButton_13">
|
||||||
|
<property name="toolTip">
|
||||||
|
<string>Reset all values to GCS defaults</string>
|
||||||
|
</property>
|
||||||
|
<property name="styleSheet">
|
||||||
|
<string notr="true"/>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string>Default</string>
|
||||||
|
</property>
|
||||||
|
<property name="objrelation" stdset="0">
|
||||||
|
<stringlist>
|
||||||
|
<string>objname:StabilizationSettings</string>
|
||||||
|
<string>button:default</string>
|
||||||
|
<string>buttongroup:55</string>
|
||||||
|
</stringlist>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<widget class="QGroupBox" name="groupBox_13">
|
||||||
|
<property name="title">
|
||||||
|
<string>FPV Camera Tilt Compensation</string>
|
||||||
|
</property>
|
||||||
|
<layout class="QGridLayout" name="gridLayout_33">
|
||||||
|
<property name="leftMargin">
|
||||||
|
<number>9</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="text">
|
<property name="topMargin">
|
||||||
<string>Enable pirouette compensation</string>
|
<number>9</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="objrelation" stdset="0">
|
<property name="rightMargin">
|
||||||
<stringlist>
|
<number>9</number>
|
||||||
<string>objname:StabilizationSettingsBankX</string>
|
|
||||||
<string>fieldname:EnablePiroComp</string>
|
|
||||||
<string>buttongroup:55</string>
|
|
||||||
</stringlist>
|
|
||||||
</property>
|
</property>
|
||||||
</widget>
|
<property name="bottomMargin">
|
||||||
</item>
|
<number>9</number>
|
||||||
<item row="0" column="0" alignment="Qt::AlignRight">
|
|
||||||
<widget class="QPushButton" name="pushButton_13">
|
|
||||||
<property name="toolTip">
|
|
||||||
<string>Reset all values to GCS defaults</string>
|
|
||||||
</property>
|
</property>
|
||||||
<property name="styleSheet">
|
<item row="0" column="2" alignment="Qt::AlignRight|Qt::AlignVCenter">
|
||||||
<string notr="true"/>
|
<widget class="QPushButton" name="pushButton_14">
|
||||||
</property>
|
<property name="toolTip">
|
||||||
<property name="text">
|
<string>Reset value to GCS defaults</string>
|
||||||
<string>Default</string>
|
</property>
|
||||||
</property>
|
<property name="styleSheet">
|
||||||
<property name="objrelation" stdset="0">
|
<string notr="true"/>
|
||||||
<stringlist>
|
</property>
|
||||||
<string>objname:StabilizationSettings</string>
|
<property name="text">
|
||||||
<string>button:default</string>
|
<string>Default</string>
|
||||||
<string>buttongroup:55</string>
|
</property>
|
||||||
</stringlist>
|
<property name="objrelation" stdset="0">
|
||||||
</property>
|
<stringlist>
|
||||||
</widget>
|
<string>objname:StabilizationSettings</string>
|
||||||
</item>
|
<string>button:default</string>
|
||||||
</layout>
|
<string>buttongroup:56</string>
|
||||||
</widget>
|
</stringlist>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="2" column="0">
|
||||||
|
<widget class="QLabel" name="label_13">
|
||||||
|
<property name="text">
|
||||||
|
<string>Camera Tilt Angle (deg)</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="2" column="1">
|
||||||
|
<widget class="QDoubleSpinBox" name="FpvCamTiltCompensation">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>80</width>
|
||||||
|
<height>22</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="maximumSize">
|
||||||
|
<size>
|
||||||
|
<width>80</width>
|
||||||
|
<height>22</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="focusPolicy">
|
||||||
|
<enum>Qt::StrongFocus</enum>
|
||||||
|
</property>
|
||||||
|
<property name="toolTip">
|
||||||
|
<string>Camera tilt angle from 0 to 50 degrees (0 to disable compensation).</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||||
|
</property>
|
||||||
|
<property name="accelerated">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
<property name="decimals">
|
||||||
|
<number>0</number>
|
||||||
|
</property>
|
||||||
|
<property name="minimum">
|
||||||
|
<double>0.000000000000000</double>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<double>50.000000000000000</double>
|
||||||
|
</property>
|
||||||
|
<property name="value">
|
||||||
|
<double>0.000000000000000</double>
|
||||||
|
</property>
|
||||||
|
<property name="objrelation" stdset="0">
|
||||||
|
<stringlist>
|
||||||
|
<string>objname:StabilizationSettingsBankX</string>
|
||||||
|
<string>fieldname:FpvCamTiltCompensation</string>
|
||||||
|
<string>haslimits:no</string>
|
||||||
|
<string>scale:1</string>
|
||||||
|
<string>buttongroup:56</string>
|
||||||
|
</stringlist>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="2" column="2">
|
||||||
|
<spacer name="horizontalSpacer_4">
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="sizeType">
|
||||||
|
<enum>QSizePolicy::Minimum</enum>
|
||||||
|
</property>
|
||||||
|
<property name="sizeHint" stdset="0">
|
||||||
|
<size>
|
||||||
|
<width>40</width>
|
||||||
|
<height>20</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
</spacer>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
</item>
|
</item>
|
||||||
</layout>
|
</layout>
|
||||||
</widget>
|
</widget>
|
||||||
@ -26113,19 +26242,18 @@ Useful if you have accidentally changed some settings.</string>
|
|||||||
</customwidget>
|
</customwidget>
|
||||||
</customwidgets>
|
</customwidgets>
|
||||||
<tabstops>
|
<tabstops>
|
||||||
<tabstop>tabWidget</tabstop>
|
|
||||||
<tabstop>scrollArea</tabstop>
|
|
||||||
<tabstop>basicResponsivenessCheckBox</tabstop>
|
<tabstop>basicResponsivenessCheckBox</tabstop>
|
||||||
<tabstop>pushButton</tabstop>
|
|
||||||
<tabstop>AttitudeResponsivenessSlider</tabstop>
|
|
||||||
<tabstop>spinBox</tabstop>
|
|
||||||
<tabstop>RateResponsivenessSlider</tabstop>
|
<tabstop>RateResponsivenessSlider</tabstop>
|
||||||
|
<tabstop>spinBox</tabstop>
|
||||||
<tabstop>spinBox_2</tabstop>
|
<tabstop>spinBox_2</tabstop>
|
||||||
<tabstop>RateYawResponsivenessSlider</tabstop>
|
<tabstop>RateYawResponsivenessSlider</tabstop>
|
||||||
<tabstop>spinBox_5</tabstop>
|
<tabstop>spinBox_5</tabstop>
|
||||||
<tabstop>pushButton_21</tabstop>
|
<tabstop>pushButton_21</tabstop>
|
||||||
|
<tabstop>pushButton</tabstop>
|
||||||
|
<tabstop>FpvCamTiltCompensation</tabstop>
|
||||||
<tabstop>AcroFactorRollSlider</tabstop>
|
<tabstop>AcroFactorRollSlider</tabstop>
|
||||||
<tabstop>AcroFactorRollSpinBox</tabstop>
|
<tabstop>AcroFactorRollSpinBox</tabstop>
|
||||||
|
<tabstop>pushButton_14</tabstop>
|
||||||
<tabstop>AcroFactorPitchSlider</tabstop>
|
<tabstop>AcroFactorPitchSlider</tabstop>
|
||||||
<tabstop>AcroFactorPitchSpinBox</tabstop>
|
<tabstop>AcroFactorPitchSpinBox</tabstop>
|
||||||
<tabstop>pushButton_12</tabstop>
|
<tabstop>pushButton_12</tabstop>
|
||||||
@ -26234,6 +26362,14 @@ Useful if you have accidentally changed some settings.</string>
|
|||||||
<tabstop>stabilizationReloadBoardData_6</tabstop>
|
<tabstop>stabilizationReloadBoardData_6</tabstop>
|
||||||
<tabstop>saveStabilizationToRAM_6</tabstop>
|
<tabstop>saveStabilizationToRAM_6</tabstop>
|
||||||
<tabstop>saveStabilizationToSD_6</tabstop>
|
<tabstop>saveStabilizationToSD_6</tabstop>
|
||||||
|
<tabstop>checkBoxLinkAcroFactors</tabstop>
|
||||||
|
<tabstop>tabWidget</tabstop>
|
||||||
|
<tabstop>AttitudeResponsivenessSlider</tabstop>
|
||||||
|
<tabstop>scrollArea</tabstop>
|
||||||
|
<tabstop>VelKdSlider</tabstop>
|
||||||
|
<tabstop>VelKd</tabstop>
|
||||||
|
<tabstop>VelBetaSlider</tabstop>
|
||||||
|
<tabstop>VelBeta</tabstop>
|
||||||
</tabstops>
|
</tabstops>
|
||||||
<resources>
|
<resources>
|
||||||
<include location="../coreplugin/core.qrc"/>
|
<include location="../coreplugin/core.qrc"/>
|
||||||
|
|||||||
@ -20,6 +20,8 @@
|
|||||||
|
|
||||||
<field name="EnablePiroComp" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
|
<field name="EnablePiroComp" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
|
||||||
|
|
||||||
|
<field name="FpvCamTiltCompensation" units="deg" type="uint8" elements="1" defaultvalue="0"/>
|
||||||
|
|
||||||
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||||
<field name="ThrustPIDScaleCurve" units="percent" type="int8" elementnames="0,25,50,75,100" defaultvalue="30,15,0,-15,-30"/>
|
<field name="ThrustPIDScaleCurve" units="percent" type="int8" elementnames="0,25,50,75,100" defaultvalue="30,15,0,-15,-30"/>
|
||||||
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
|
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
|
||||||
|
|||||||
@ -20,6 +20,8 @@
|
|||||||
|
|
||||||
<field name="EnablePiroComp" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
|
<field name="EnablePiroComp" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
|
||||||
|
|
||||||
|
<field name="FpvCamTiltCompensation" units="deg" type="uint8" elements="1" defaultvalue="0"/>
|
||||||
|
|
||||||
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||||
<field name="ThrustPIDScaleCurve" units="percent" type="int8" elementnames="0,25,50,75,100" defaultvalue="30,15,0,-15,-30"/>
|
<field name="ThrustPIDScaleCurve" units="percent" type="int8" elementnames="0,25,50,75,100" defaultvalue="30,15,0,-15,-30"/>
|
||||||
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
|
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
|
||||||
|
|||||||
@ -20,6 +20,8 @@
|
|||||||
|
|
||||||
<field name="EnablePiroComp" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
|
<field name="EnablePiroComp" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
|
||||||
|
|
||||||
|
<field name="FpvCamTiltCompensation" units="deg" type="uint8" elements="1" defaultvalue="0"/>
|
||||||
|
|
||||||
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||||
<field name="ThrustPIDScaleCurve" units="percent" type="int8" elementnames="0,25,50,75,100" defaultvalue="30,15,0,-15,-30"/>
|
<field name="ThrustPIDScaleCurve" units="percent" type="int8" elementnames="0,25,50,75,100" defaultvalue="30,15,0,-15,-30"/>
|
||||||
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
|
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
|
||||||
|
|||||||
@ -20,6 +20,8 @@
|
|||||||
|
|
||||||
<field name="EnablePiroComp" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
|
<field name="EnablePiroComp" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
|
||||||
|
|
||||||
|
<field name="FpvCamTiltCompensation" units="deg" type="uint8" elements="1" defaultvalue="0"/>
|
||||||
|
|
||||||
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||||
<field name="ThrustPIDScaleCurve" units="percent" type="int8" elementnames="0,25,50,75,100" defaultvalue="30,15,0,-15,-30"/>
|
<field name="ThrustPIDScaleCurve" units="percent" type="int8" elementnames="0,25,50,75,100" defaultvalue="30,15,0,-15,-30"/>
|
||||||
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
|
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user