From 493fcaef2a8a41af2380963b276a72e69eb6737e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 23 Jun 2011 00:46:41 -0500 Subject: [PATCH] OP-193 OP-511: Add settings to scale the camera input output range --- flight/CopterControl/Makefile | 5 +- flight/Modules/CameraStab/camerastab.c | 60 +++++++++++++------ .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 + .../src/plugins/uavobjects/uavobjects.pro | 2 + 4 files changed, 49 insertions(+), 20 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index e05420f90..8a612aab9 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -168,6 +168,7 @@ SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c SRC += $(OPUAVSYNTHDIR)/mixersettings.c SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c +SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c ifeq ($(DIAGNOISTCS),YES) SRC += $(OPUAVSYNTHDIR)/taskinfo.c @@ -438,7 +439,7 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. #CFLAGS += -fno-cprop-registers -fno-defer-pop -fno-guess-branch-probability -fno-section-anchors #CFLAGS += -fno-if-conversion -fno-if-conversion2 -fno-ipa-pure-const -fno-ipa-reference -fno-merge-constants -#CFLAGS += -fno-split-wide-types -fno-tree-ccp -fno-tree-ch -fno-tree-copy-prop -fno-tree-copyrename +#CFLAGS += -fno-split-wide-types -fno-tree-ccp -fno-tree-ch -fno-tree-copy-prop -fno-tree-copyrename #CFLAGS += -fno-tree-dce -fno-tree-dominator-opts -fno-tree-dse -fno-tree-fre -fno-tree-sink -fno-tree-sra #CFLAGS += -fno-tree-ter #CFLAGS += -g$(DEBUGF) -DDEBUG @@ -529,7 +530,7 @@ ${OUTDIR}/InitMods.c: Makefile @echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c # Generate code for PyMite -#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py) +#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py) # @echo $(MSG_PYMITEINIT) $(call toprel, $@) # @$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py) # @$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c index c82a14759..4682a7ed0 100644 --- a/flight/Modules/CameraStab/camerastab.c +++ b/flight/Modules/CameraStab/camerastab.c @@ -49,11 +49,12 @@ #include "accessorydesired.h" #include "attitudeactual.h" +#include "camerastabsettings.h" // // Configuration // -#define SAMPLE_PERIOD_MS 50 +#define SAMPLE_PERIOD_MS 10 // Private types @@ -73,6 +74,8 @@ int32_t CameraStabInitialize(void) ev.instId = 0; ev.event = 0; + CameraStabSettingsInitialize(); + EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); return 0; @@ -85,25 +88,46 @@ static void attitudeUpdated(UAVObjEvent* ev) float attitude; AccessoryDesiredData accessory; - float rollConst = 1.0/35.0; - float pitchConst = 1.0/35.0; - float yawConst = 1.0/35.0; - - - AttitudeActualRollGet(&attitude); - accessory.AccessoryVal = attitude * rollConst; - if(AccessoryDesiredInstSet(0, &accessory) != 0) - AccessoryDesiredCreateInstance(); - AttitudeActualPitchGet(&attitude); - accessory.AccessoryVal = attitude * pitchConst; - if(AccessoryDesiredInstSet(1, &accessory) != 0) - AccessoryDesiredCreateInstance(); + CameraStabSettingsData cameraStab; + CameraStabSettingsGet(&cameraStab); - AttitudeActualYawGet(&attitude); - accessory.AccessoryVal = attitude * yawConst; - if(AccessoryDesiredInstSet(2, &accessory) != 0) - AccessoryDesiredCreateInstance(); + // Read any input channels + float inputs[3] = {0,0,0}; + if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] != CAMERASTABSETTINGS_INPUTS_NONE) { + if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) != 0) + inputs[0] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_ROLL]; + } + if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] != CAMERASTABSETTINGS_INPUTS_NONE) { + if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) != 0) + inputs[1] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_PITCH]; + } + if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] != CAMERASTABSETTINGS_INPUTS_NONE) { + if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) != 0) + inputs[2] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_YAW]; + } + + // Set output channels + if(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_ROLL] != CAMERASTABSETTINGS_OUTPUTS_NONE) { + AttitudeActualRollGet(&attitude); + accessory.AccessoryVal = (attitude + inputs[0]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; + if(AccessoryDesiredInstSet(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_ROLL] - CAMERASTABSETTINGS_OUTPUTS_ACCESSORY0, &accessory) != 0) + AccessoryDesiredCreateInstance(); + } + + if(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_PITCH] != CAMERASTABSETTINGS_OUTPUTS_NONE) { + AttitudeActualPitchGet(&attitude); + accessory.AccessoryVal = (attitude + inputs[1]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; + if(AccessoryDesiredInstSet(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_PITCH] - CAMERASTABSETTINGS_OUTPUTS_ACCESSORY0, &accessory) != 0) + AccessoryDesiredCreateInstance(); + } + + if(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_YAW] != CAMERASTABSETTINGS_OUTPUTS_NONE) { + AttitudeActualYawGet(&attitude); + accessory.AccessoryVal = (attitude + inputs[2]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW]; + if(AccessoryDesiredInstSet(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_YAW] - CAMERASTABSETTINGS_OUTPUTS_ACCESSORY0, &accessory) != 0) + AccessoryDesiredCreateInstance(); + } } /** diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 009d9cfb9..a5503a27f 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -85,6 +85,7 @@ 6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = ""; }; 6549E0D31279B3CF00C5476F /* fifo_buffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = fifo_buffer.h; sourceTree = ""; }; 655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = ""; }; + 655B1A8E13B2FC0900B0E48D /* camerastabsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = camerastabsettings.xml; sourceTree = ""; }; 656268C612DC1923007B0A0F /* nedaccel.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = nedaccel.xml; sourceTree = ""; }; 65632DF51251650300469B77 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = ""; }; 65632DF61251650300469B77 /* STM32103CB_AHRS.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM32103CB_AHRS.h; sourceTree = ""; }; @@ -7434,6 +7435,7 @@ 65E410AE12F65AEA00725888 /* attitudesettings.xml */, 65C35E5912EFB2F3004811C2 /* baroaltitude.xml */, 65C35E5A12EFB2F3004811C2 /* batterysettings.xml */, + 655B1A8E13B2FC0900B0E48D /* camerastabsettings.xml */, 652C8568132B632A00BFCC70 /* firmwareiapobj.xml */, 65C35E5C12EFB2F3004811C2 /* flightbatterystate.xml */, 65C35E5D12EFB2F3004811C2 /* flightplancontrol.xml */, diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 8bd7420be..31cd6b78a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -31,6 +31,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/ahrssettings.h \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/attituderaw.h \ + $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/systemstats.h \ $$UAVOBJECT_SYNTHETICS/systemalarms.h \ @@ -78,6 +79,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/ahrssettings.cpp \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/attituderaw.cpp \ + $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/systemstats.cpp \ $$UAVOBJECT_SYNTHETICS/systemalarms.cpp \