diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c new file mode 100644 index 000000000..c82a14759 --- /dev/null +++ b/flight/Modules/CameraStab/camerastab.c @@ -0,0 +1,115 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup CameraStab Camera Stabilization Module + * @brief Camera stabilization module + * Updates accessory outputs with values appropriate for camera stabilization + * @{ + * + * @file camerastab.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Stabilize camera against the roll pitch and yaw of aircraft + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * Output object: Accessory + * + * This module will periodically calculate the output values for stabilizing the camera + * + * UAVObjects are automatically generated by the UAVObjectGenerator from + * the object definition XML file. + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ + +#include "openpilot.h" + +#include "accessorydesired.h" +#include "attitudeactual.h" + +// +// Configuration +// +#define SAMPLE_PERIOD_MS 50 + +// Private types + +// Private variables + +// Private functions +static void attitudeUpdated(UAVObjEvent* ev); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t CameraStabInitialize(void) +{ + static UAVObjEvent ev; + ev.obj = AttitudeActualHandle(); + ev.instId = 0; + ev.event = 0; + + EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + + return 0; +} + +static void attitudeUpdated(UAVObjEvent* ev) +{ + if (ev->obj != AttitudeActualHandle()) + return; + + 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(); + + AttitudeActualYawGet(&attitude); + accessory.AccessoryVal = attitude * yawConst; + if(AccessoryDesiredInstSet(2, &accessory) != 0) + AccessoryDesiredCreateInstance(); +} + +/** + * @} + */ + +/** + * @} + */ diff --git a/flight/Modules/CameraStab/inc/camerastab.h b/flight/Modules/CameraStab/inc/camerastab.h new file mode 100644 index 000000000..bc2515696 --- /dev/null +++ b/flight/Modules/CameraStab/inc/camerastab.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup BatteryModule Battery Module + * @{ + * + * @file battery.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Module to read the battery Voltage and Current periodically and set alarms appropriately. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef BATTERY_H +#define BATTERY_H + +#include "openpilot.h" + +int32_t BatteryInitialize(void); + +#endif // BATTERY_H + +/** + * @} + * @} + */ diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 4f2252237..452547f2e 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -2687,6 +2687,8 @@ 65C35EA812F0A834004811C2 /* eventdispatcher.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = eventdispatcher.c; sourceTree = ""; }; 65C35F6612F0DC2D004811C2 /* attitude.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = attitude.c; sourceTree = ""; }; 65C35F6812F0DC2D004811C2 /* attitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attitude.h; sourceTree = ""; }; + 65C9903C13A871B90082BD60 /* camerastab.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = camerastab.c; sourceTree = ""; }; + 65C9903E13A871B90082BD60 /* camerastab.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = camerastab.h; sourceTree = ""; }; 65D2CA841248F9A400B1E7D6 /* mixersettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixersettings.xml; sourceTree = ""; }; 65D2CA851248F9A400B1E7D6 /* mixerstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixerstatus.xml; sourceTree = ""; }; 65E410AE12F65AEA00725888 /* attitudesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudesettings.xml; sourceTree = ""; }; @@ -3205,6 +3207,7 @@ 650D8E2812DFE16400D05CC9 /* Altitude */, 65C35F6512F0DC2D004811C2 /* Attitude */, 650D8E2E12DFE16400D05CC9 /* Battery */, + 65C9903B13A871B90082BD60 /* CameraStab */, 650D8E3212DFE16400D05CC9 /* Example */, 650D8E3B12DFE16400D05CC9 /* FirmwareIAP */, 650D8E3F12DFE16400D05CC9 /* FlightPlan */, @@ -7498,6 +7501,23 @@ path = inc; sourceTree = ""; }; + 65C9903B13A871B90082BD60 /* CameraStab */ = { + isa = PBXGroup; + children = ( + 65C9903C13A871B90082BD60 /* camerastab.c */, + 65C9903D13A871B90082BD60 /* inc */, + ); + path = CameraStab; + sourceTree = ""; + }; + 65C9903D13A871B90082BD60 /* inc */ = { + isa = PBXGroup; + children = ( + 65C9903E13A871B90082BD60 /* camerastab.h */, + ); + path = inc; + sourceTree = ""; + }; 65E6DF7012E02E8E00058553 /* CopterControl */ = { isa = PBXGroup; children = (