diff --git a/Makefile b/Makefile index 6564abc44..54cdf913a 100644 --- a/Makefile +++ b/Makefile @@ -162,7 +162,7 @@ QT_SDK_DIR := $(TOOLS_DIR)/qtsdk-v1.2 .PHONY: qt_sdk_install qt_sdk_install: QT_SDK_URL := http://www.developer.nokia.com/dp?uri=http://sw.nokia.com/id/8ea74da4-fec1-4277-8b26-c58cc82e204b/Qt_SDK_Lin32_offline -qt_sdk_install: QT_SDK_FILE := Qt_SDK_Lin32_offline_v1_2_en.run +qt_sdk_install: QT_SDK_FILE := QtSdk-offline-linux-x86-v1.2.1.run #qt_sdk_install: QT_SDK_URL := http://www.developer.nokia.com/dp?uri=http://sw.nokia.com/id/c365bbf5-c2b9-4dda-9c1f-34b2c8d07785/Qt_SDK_Lin32_offline_v1_1_2 #qt_sdk_install: QT_SDK_FILE := Qt_SDK_Lin32_offline_v1_1_2_en.run # order-only prereq on directory existance: @@ -433,7 +433,7 @@ dfuutil_clean: ############################## ifeq ($(shell [ -d "$(QT_SDK_DIR)" ] && echo "exists"), exists) - QMAKE=$(QT_SDK_DIR)/Desktop/Qt/4.8.0/gcc/bin/qmake + QMAKE=$(QT_SDK_DIR)/Desktop/Qt/4.8.1/gcc/bin/qmake else # not installed, hope it's in the path... QMAKE=qmake diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/FlightStatus.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/FlightStatus.java index cdf2f39f2..97c0fafe5 100644 --- a/androidgcs/src/org/openpilot/uavtalk/uavobjects/FlightStatus.java +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/FlightStatus.java @@ -66,6 +66,7 @@ public class FlightStatus extends UAVDataObject { FlightModeEnumOptions.add("Stabilized1"); FlightModeEnumOptions.add("Stabilized2"); FlightModeEnumOptions.add("Stabilized3"); + FlightModeEnumOptions.add("Autotune"); FlightModeEnumOptions.add("AltitudeHold"); FlightModeEnumOptions.add("VelocityControl"); FlightModeEnumOptions.add("PositionHold"); @@ -144,7 +145,7 @@ public class FlightStatus extends UAVDataObject { } // Constants - protected static final long OBJID = 0xE7A476DAl; + protected static final long OBJID = 0x9B6A127El; protected static final String NAME = "FlightStatus"; protected static String DESCRIPTION = "Contains major flight status information for other modules."; protected static final boolean ISSINGLEINST = 1 > 0; diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/GPSSettings.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/GPSSettings.java index 068dc34f3..317d4d249 100644 --- a/androidgcs/src/org/openpilot/uavtalk/uavobjects/GPSSettings.java +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/GPSSettings.java @@ -102,7 +102,7 @@ public class GPSSettings extends UAVDataObject { */ public void setDefaultFieldValues() { - getField("DataProtocol").setValue("NMEA"); + getField("DataProtocol").setValue("UBX"); } diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/HwSettings.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/HwSettings.java index 2d24ffeeb..a6716d209 100644 --- a/androidgcs/src/org/openpilot/uavtalk/uavobjects/HwSettings.java +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/HwSettings.java @@ -194,6 +194,7 @@ public class HwSettings extends UAVDataObject { USB_HIDPortElemNames.add("0"); List USB_HIDPortEnumOptions = new ArrayList(); USB_HIDPortEnumOptions.add("USBTelemetry"); + USB_HIDPortEnumOptions.add("RCTransmitter"); USB_HIDPortEnumOptions.add("Disabled"); fields.add( new UAVObjectField("USB_HIDPort", "function", UAVObjectField.FieldType.ENUM, USB_HIDPortElemNames, USB_HIDPortEnumOptions) ); @@ -212,6 +213,7 @@ public class HwSettings extends UAVDataObject { OptionalModulesElemNames.add("Fault"); OptionalModulesElemNames.add("Altitude"); OptionalModulesElemNames.add("TxPID"); + OptionalModulesElemNames.add("Autotune"); List OptionalModulesEnumOptions = new ArrayList(); OptionalModulesEnumOptions.add("Disabled"); OptionalModulesEnumOptions.add("Enabled"); @@ -266,7 +268,7 @@ public class HwSettings extends UAVDataObject { public void setDefaultFieldValues() { getField("CC_RcvrPort").setValue("PWM"); - getField("CC_MainPort").setValue("Disabled"); + getField("CC_MainPort").setValue("Telemetry"); getField("CC_FlexiPort").setValue("Disabled"); getField("RV_RcvrPort").setValue("PWM"); getField("RV_AuxPort").setValue("Disabled"); @@ -285,6 +287,7 @@ public class HwSettings extends UAVDataObject { getField("OptionalModules").setValue("Disabled",3); getField("OptionalModules").setValue("Disabled",4); getField("OptionalModules").setValue("Disabled",5); + getField("OptionalModules").setValue("Disabled",6); getField("DSMxBind").setValue(0); } @@ -314,7 +317,7 @@ public class HwSettings extends UAVDataObject { } // Constants - protected static final long OBJID = 0x9408E9F0l; + protected static final long OBJID = 0x428D4DCEl; protected static final String NAME = "HwSettings"; protected static String DESCRIPTION = "Selection of optional hardware configurations."; protected static final boolean ISSINGLEINST = 1 > 0; diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/ManualControlSettings.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/ManualControlSettings.java index afdd204b4..be71d23f0 100644 --- a/androidgcs/src/org/openpilot/uavtalk/uavobjects/ManualControlSettings.java +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/ManualControlSettings.java @@ -151,6 +151,8 @@ public class ManualControlSettings extends UAVDataObject { Stabilization1SettingsEnumOptions.add("AxisLock"); Stabilization1SettingsEnumOptions.add("WeakLeveling"); Stabilization1SettingsEnumOptions.add("VirtualBar"); + Stabilization1SettingsEnumOptions.add("RelayRate"); + Stabilization1SettingsEnumOptions.add("RelayAttitude"); fields.add( new UAVObjectField("Stabilization1Settings", "", UAVObjectField.FieldType.ENUM, Stabilization1SettingsElemNames, Stabilization1SettingsEnumOptions) ); List Stabilization2SettingsElemNames = new ArrayList(); @@ -164,6 +166,8 @@ public class ManualControlSettings extends UAVDataObject { Stabilization2SettingsEnumOptions.add("AxisLock"); Stabilization2SettingsEnumOptions.add("WeakLeveling"); Stabilization2SettingsEnumOptions.add("VirtualBar"); + Stabilization2SettingsEnumOptions.add("RelayRate"); + Stabilization2SettingsEnumOptions.add("RelayAttitude"); fields.add( new UAVObjectField("Stabilization2Settings", "", UAVObjectField.FieldType.ENUM, Stabilization2SettingsElemNames, Stabilization2SettingsEnumOptions) ); List Stabilization3SettingsElemNames = new ArrayList(); @@ -177,6 +181,8 @@ public class ManualControlSettings extends UAVDataObject { Stabilization3SettingsEnumOptions.add("AxisLock"); Stabilization3SettingsEnumOptions.add("WeakLeveling"); Stabilization3SettingsEnumOptions.add("VirtualBar"); + Stabilization3SettingsEnumOptions.add("RelayRate"); + Stabilization3SettingsEnumOptions.add("RelayAttitude"); fields.add( new UAVObjectField("Stabilization3Settings", "", UAVObjectField.FieldType.ENUM, Stabilization3SettingsElemNames, Stabilization3SettingsEnumOptions) ); List FlightModeNumberElemNames = new ArrayList(); @@ -195,6 +201,7 @@ public class ManualControlSettings extends UAVDataObject { FlightModePositionEnumOptions.add("Stabilized1"); FlightModePositionEnumOptions.add("Stabilized2"); FlightModePositionEnumOptions.add("Stabilized3"); + FlightModePositionEnumOptions.add("Autotune"); FlightModePositionEnumOptions.add("AltitudeHold"); FlightModePositionEnumOptions.add("VelocityControl"); FlightModePositionEnumOptions.add("PositionHold"); @@ -306,8 +313,8 @@ public class ManualControlSettings extends UAVDataObject { getField("FlightModePosition").setValue("Stabilized1",1); getField("FlightModePosition").setValue("Stabilized2",2); getField("FlightModePosition").setValue("Stabilized3",3); - getField("FlightModePosition").setValue("AltitudeHold",4); - getField("FlightModePosition").setValue("PositionHold",5); + getField("FlightModePosition").setValue("Stabilized1",4); + getField("FlightModePosition").setValue("Stabilized2",5); } @@ -336,7 +343,7 @@ public class ManualControlSettings extends UAVDataObject { } // Constants - protected static final long OBJID = 0x7672339El; + protected static final long OBJID = 0xA240D466l; protected static final String NAME = "ManualControlSettings"; protected static String DESCRIPTION = "Settings to indicate how to decode receiver input by @ref ManualControlModule."; protected static final boolean ISSINGLEINST = 1 > 0; diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/PathAction.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/PathAction.java new file mode 100644 index 000000000..f21e301c3 --- /dev/null +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/PathAction.java @@ -0,0 +1,195 @@ +/** + ****************************************************************************** + * + * @file uavobjecttemplate.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Template for an uavobject in java + * This is a autogenerated file!! Do not modify and expect a result. + * A waypoint command the pathplanner is to use at a certain waypoint + * + * @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 + */ + +package org.openpilot.uavtalk.uavobjects; + +import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.List; +import java.util.ListIterator; + +import org.openpilot.uavtalk.UAVObjectManager; +import org.openpilot.uavtalk.UAVObject; +import org.openpilot.uavtalk.UAVDataObject; +import org.openpilot.uavtalk.UAVObjectField; + +/** +A waypoint command the pathplanner is to use at a certain waypoint + +generated from pathaction.xml + **/ +public class PathAction extends UAVDataObject { + + public PathAction() { + super(OBJID, ISSINGLEINST, ISSETTINGS, NAME); + + List fields = new ArrayList(); + + + List ModeParametersElemNames = new ArrayList(); + ModeParametersElemNames.add("0"); + ModeParametersElemNames.add("1"); + ModeParametersElemNames.add("2"); + ModeParametersElemNames.add("3"); + fields.add( new UAVObjectField("ModeParameters", "", UAVObjectField.FieldType.FLOAT32, ModeParametersElemNames, null) ); + + List ConditionParametersElemNames = new ArrayList(); + ConditionParametersElemNames.add("0"); + ConditionParametersElemNames.add("1"); + ConditionParametersElemNames.add("2"); + ConditionParametersElemNames.add("3"); + fields.add( new UAVObjectField("ConditionParameters", "", UAVObjectField.FieldType.FLOAT32, ConditionParametersElemNames, null) ); + + List ModeElemNames = new ArrayList(); + ModeElemNames.add("0"); + List ModeEnumOptions = new ArrayList(); + ModeEnumOptions.add("FlyEndpoint"); + ModeEnumOptions.add("FlyVector"); + ModeEnumOptions.add("FlyCircleRight"); + ModeEnumOptions.add("FlyCircleLeft"); + ModeEnumOptions.add("DriveEndpoint"); + ModeEnumOptions.add("DriveVector"); + ModeEnumOptions.add("DriveCircleLeft"); + ModeEnumOptions.add("DriveCircleRight"); + ModeEnumOptions.add("FixedAttitude"); + ModeEnumOptions.add("SetAccessory"); + ModeEnumOptions.add("DisarmAlarm"); + fields.add( new UAVObjectField("Mode", "", UAVObjectField.FieldType.ENUM, ModeElemNames, ModeEnumOptions) ); + + List EndConditionElemNames = new ArrayList(); + EndConditionElemNames.add("0"); + List EndConditionEnumOptions = new ArrayList(); + EndConditionEnumOptions.add("None"); + EndConditionEnumOptions.add("TimeOut"); + EndConditionEnumOptions.add("DistanceToTarget"); + EndConditionEnumOptions.add("LegRemaining"); + EndConditionEnumOptions.add("AboveAltitude"); + EndConditionEnumOptions.add("PointingTowardsNext"); + EndConditionEnumOptions.add("PythonScript"); + EndConditionEnumOptions.add("Immediate"); + fields.add( new UAVObjectField("EndCondition", "", UAVObjectField.FieldType.ENUM, EndConditionElemNames, EndConditionEnumOptions) ); + + List CommandElemNames = new ArrayList(); + CommandElemNames.add("0"); + List CommandEnumOptions = new ArrayList(); + CommandEnumOptions.add("OnConditionNextWaypoint"); + CommandEnumOptions.add("OnNotConditionNextWaypoint"); + CommandEnumOptions.add("OnConditionJumpWaypoint"); + CommandEnumOptions.add("OnNotConditionJumpWaypoint"); + CommandEnumOptions.add("IfConditionJumpWaypointElseNextWaypoint"); + fields.add( new UAVObjectField("Command", "", UAVObjectField.FieldType.ENUM, CommandElemNames, CommandEnumOptions) ); + + List JumpDestinationElemNames = new ArrayList(); + JumpDestinationElemNames.add("0"); + fields.add( new UAVObjectField("JumpDestination", "waypoint", UAVObjectField.FieldType.UINT8, JumpDestinationElemNames, null) ); + + List ErrorDestinationElemNames = new ArrayList(); + ErrorDestinationElemNames.add("0"); + fields.add( new UAVObjectField("ErrorDestination", "waypoint", UAVObjectField.FieldType.UINT8, ErrorDestinationElemNames, null) ); + + + // Compute the number of bytes for this object + int numBytes = 0; + ListIterator li = fields.listIterator(); + while(li.hasNext()) { + numBytes += li.next().getNumBytes(); + } + NUMBYTES = numBytes; + + // Initialize object + initializeFields(fields, ByteBuffer.allocate(NUMBYTES), NUMBYTES); + // Set the default field values + setDefaultFieldValues(); + // Set the object description + setDescription(DESCRIPTION); + } + + /** + * Create a Metadata object filled with default values for this object + * @return Metadata object with default values + */ + public Metadata getDefaultMetadata() { + UAVObject.Metadata metadata = new UAVObject.Metadata(); + metadata.flags = + UAVObject.Metadata.AccessModeNum(UAVObject.AccessMode.ACCESS_READWRITE) << UAVOBJ_ACCESS_SHIFT | + UAVObject.Metadata.AccessModeNum(UAVObject.AccessMode.ACCESS_READWRITE) << UAVOBJ_GCS_ACCESS_SHIFT | + 0 << UAVOBJ_TELEMETRY_ACKED_SHIFT | + 0 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | + UAVObject.Metadata.UpdateModeNum(UAVObject.UpdateMode.UPDATEMODE_PERIODIC) << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | + UAVObject.Metadata.UpdateModeNum(UAVObject.UpdateMode.UPDATEMODE_MANUAL) << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT; + metadata.flightTelemetryUpdatePeriod = 4000; + metadata.gcsTelemetryUpdatePeriod = 0; + metadata.loggingUpdatePeriod = 1000; + + return metadata; + } + + /** + * Initialize object fields with the default values. + * If a default value is not specified the object fields + * will be initialized to zero. + */ + public void setDefaultFieldValues() + { + + } + + /** + * Create a clone of this object, a new instance ID must be specified. + * Do not use this function directly to create new instances, the + * UAVObjectManager should be used instead. + */ + public UAVDataObject clone(long instID) { + // TODO: Need to get specific instance to clone + try { + PathAction obj = new PathAction(); + obj.initialize(instID, this.getMetaObject()); + return obj; + } catch (Exception e) { + return null; + } + } + + /** + * Static function to retrieve an instance of the object. + */ + public PathAction GetInstance(UAVObjectManager objMngr, long instID) + { + return (PathAction)(objMngr.getObject(PathAction.OBJID, instID)); + } + + // Constants + protected static final long OBJID = 0x34595F1Cl; + protected static final String NAME = "PathAction"; + protected static String DESCRIPTION = "A waypoint command the pathplanner is to use at a certain waypoint"; + protected static final boolean ISSINGLEINST = 0 > 0; + protected static final boolean ISSETTINGS = 0 > 0; + protected static int NUMBYTES = 0; + + +} diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/RelayTuning.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/RelayTuning.java new file mode 100644 index 000000000..741e024a7 --- /dev/null +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/RelayTuning.java @@ -0,0 +1,146 @@ +/** + ****************************************************************************** + * + * @file uavobjecttemplate.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Template for an uavobject in java + * This is a autogenerated file!! Do not modify and expect a result. + * The input to the relay tuning. + * + * @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 + */ + +package org.openpilot.uavtalk.uavobjects; + +import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.List; +import java.util.ListIterator; + +import org.openpilot.uavtalk.UAVObjectManager; +import org.openpilot.uavtalk.UAVObject; +import org.openpilot.uavtalk.UAVDataObject; +import org.openpilot.uavtalk.UAVObjectField; + +/** +The input to the relay tuning. + +generated from relaytuning.xml + **/ +public class RelayTuning extends UAVDataObject { + + public RelayTuning() { + super(OBJID, ISSINGLEINST, ISSETTINGS, NAME); + + List fields = new ArrayList(); + + + List PeriodElemNames = new ArrayList(); + PeriodElemNames.add("Roll"); + PeriodElemNames.add("Pitch"); + PeriodElemNames.add("Yaw"); + fields.add( new UAVObjectField("Period", "ms", UAVObjectField.FieldType.FLOAT32, PeriodElemNames, null) ); + + List GainElemNames = new ArrayList(); + GainElemNames.add("Roll"); + GainElemNames.add("Pitch"); + GainElemNames.add("Yaw"); + fields.add( new UAVObjectField("Gain", "(deg/s)/output", UAVObjectField.FieldType.FLOAT32, GainElemNames, null) ); + + + // Compute the number of bytes for this object + int numBytes = 0; + ListIterator li = fields.listIterator(); + while(li.hasNext()) { + numBytes += li.next().getNumBytes(); + } + NUMBYTES = numBytes; + + // Initialize object + initializeFields(fields, ByteBuffer.allocate(NUMBYTES), NUMBYTES); + // Set the default field values + setDefaultFieldValues(); + // Set the object description + setDescription(DESCRIPTION); + } + + /** + * Create a Metadata object filled with default values for this object + * @return Metadata object with default values + */ + public Metadata getDefaultMetadata() { + UAVObject.Metadata metadata = new UAVObject.Metadata(); + metadata.flags = + UAVObject.Metadata.AccessModeNum(UAVObject.AccessMode.ACCESS_READWRITE) << UAVOBJ_ACCESS_SHIFT | + UAVObject.Metadata.AccessModeNum(UAVObject.AccessMode.ACCESS_READWRITE) << UAVOBJ_GCS_ACCESS_SHIFT | + 0 << UAVOBJ_TELEMETRY_ACKED_SHIFT | + 0 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | + UAVObject.Metadata.UpdateModeNum(UAVObject.UpdateMode.UPDATEMODE_PERIODIC) << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | + UAVObject.Metadata.UpdateModeNum(UAVObject.UpdateMode.UPDATEMODE_MANUAL) << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT; + metadata.flightTelemetryUpdatePeriod = 1000; + metadata.gcsTelemetryUpdatePeriod = 0; + metadata.loggingUpdatePeriod = 0; + + return metadata; + } + + /** + * Initialize object fields with the default values. + * If a default value is not specified the object fields + * will be initialized to zero. + */ + public void setDefaultFieldValues() + { + + } + + /** + * Create a clone of this object, a new instance ID must be specified. + * Do not use this function directly to create new instances, the + * UAVObjectManager should be used instead. + */ + public UAVDataObject clone(long instID) { + // TODO: Need to get specific instance to clone + try { + RelayTuning obj = new RelayTuning(); + obj.initialize(instID, this.getMetaObject()); + return obj; + } catch (Exception e) { + return null; + } + } + + /** + * Static function to retrieve an instance of the object. + */ + public RelayTuning GetInstance(UAVObjectManager objMngr, long instID) + { + return (RelayTuning)(objMngr.getObject(RelayTuning.OBJID, instID)); + } + + // Constants + protected static final long OBJID = 0xF6EE61BEl; + protected static final String NAME = "RelayTuning"; + protected static String DESCRIPTION = "The input to the relay tuning."; + protected static final boolean ISSINGLEINST = 1 > 0; + protected static final boolean ISSETTINGS = 0 > 0; + protected static int NUMBYTES = 0; + + +} diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/RelayTuningSettings.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/RelayTuningSettings.java new file mode 100644 index 000000000..9dfa78654 --- /dev/null +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/RelayTuningSettings.java @@ -0,0 +1,171 @@ +/** + ****************************************************************************** + * + * @file uavobjecttemplate.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Template for an uavobject in java + * This is a autogenerated file!! Do not modify and expect a result. + * Setting to run a relay tuning algorithm + * + * @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 + */ + +package org.openpilot.uavtalk.uavobjects; + +import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.List; +import java.util.ListIterator; + +import org.openpilot.uavtalk.UAVObjectManager; +import org.openpilot.uavtalk.UAVObject; +import org.openpilot.uavtalk.UAVDataObject; +import org.openpilot.uavtalk.UAVObjectField; + +/** +Setting to run a relay tuning algorithm + +generated from relaytuningsettings.xml + **/ +public class RelayTuningSettings extends UAVDataObject { + + public RelayTuningSettings() { + super(OBJID, ISSINGLEINST, ISSETTINGS, NAME); + + List fields = new ArrayList(); + + + List RateGainElemNames = new ArrayList(); + RateGainElemNames.add("0"); + fields.add( new UAVObjectField("RateGain", "", UAVObjectField.FieldType.FLOAT32, RateGainElemNames, null) ); + + List AttitudeGainElemNames = new ArrayList(); + AttitudeGainElemNames.add("0"); + fields.add( new UAVObjectField("AttitudeGain", "", UAVObjectField.FieldType.FLOAT32, AttitudeGainElemNames, null) ); + + List AmplitudeElemNames = new ArrayList(); + AmplitudeElemNames.add("0"); + fields.add( new UAVObjectField("Amplitude", "", UAVObjectField.FieldType.FLOAT32, AmplitudeElemNames, null) ); + + List HysteresisThreshElemNames = new ArrayList(); + HysteresisThreshElemNames.add("0"); + fields.add( new UAVObjectField("HysteresisThresh", "deg/s", UAVObjectField.FieldType.UINT8, HysteresisThreshElemNames, null) ); + + List ModeElemNames = new ArrayList(); + ModeElemNames.add("0"); + List ModeEnumOptions = new ArrayList(); + ModeEnumOptions.add("Rate"); + ModeEnumOptions.add("Attitude"); + fields.add( new UAVObjectField("Mode", "", UAVObjectField.FieldType.ENUM, ModeElemNames, ModeEnumOptions) ); + + List BehaviorElemNames = new ArrayList(); + BehaviorElemNames.add("0"); + List BehaviorEnumOptions = new ArrayList(); + BehaviorEnumOptions.add("Measure"); + BehaviorEnumOptions.add("Compute"); + BehaviorEnumOptions.add("Save"); + fields.add( new UAVObjectField("Behavior", "", UAVObjectField.FieldType.ENUM, BehaviorElemNames, BehaviorEnumOptions) ); + + + // Compute the number of bytes for this object + int numBytes = 0; + ListIterator li = fields.listIterator(); + while(li.hasNext()) { + numBytes += li.next().getNumBytes(); + } + NUMBYTES = numBytes; + + // Initialize object + initializeFields(fields, ByteBuffer.allocate(NUMBYTES), NUMBYTES); + // Set the default field values + setDefaultFieldValues(); + // Set the object description + setDescription(DESCRIPTION); + } + + /** + * Create a Metadata object filled with default values for this object + * @return Metadata object with default values + */ + public Metadata getDefaultMetadata() { + UAVObject.Metadata metadata = new UAVObject.Metadata(); + metadata.flags = + UAVObject.Metadata.AccessModeNum(UAVObject.AccessMode.ACCESS_READWRITE) << UAVOBJ_ACCESS_SHIFT | + UAVObject.Metadata.AccessModeNum(UAVObject.AccessMode.ACCESS_READWRITE) << UAVOBJ_GCS_ACCESS_SHIFT | + 1 << UAVOBJ_TELEMETRY_ACKED_SHIFT | + 1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT | + UAVObject.Metadata.UpdateModeNum(UAVObject.UpdateMode.UPDATEMODE_ONCHANGE) << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT | + UAVObject.Metadata.UpdateModeNum(UAVObject.UpdateMode.UPDATEMODE_ONCHANGE) << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT; + metadata.flightTelemetryUpdatePeriod = 0; + metadata.gcsTelemetryUpdatePeriod = 0; + metadata.loggingUpdatePeriod = 0; + + return metadata; + } + + /** + * Initialize object fields with the default values. + * If a default value is not specified the object fields + * will be initialized to zero. + */ + public void setDefaultFieldValues() + { + getField("RateGain").setValue(0.3333); + getField("AttitudeGain").setValue(0.2); + getField("Amplitude").setValue(0.25); + getField("HysteresisThresh").setValue(5); + getField("Mode").setValue("Attitude"); + getField("Behavior").setValue("Compute"); + + } + + /** + * Create a clone of this object, a new instance ID must be specified. + * Do not use this function directly to create new instances, the + * UAVObjectManager should be used instead. + */ + public UAVDataObject clone(long instID) { + // TODO: Need to get specific instance to clone + try { + RelayTuningSettings obj = new RelayTuningSettings(); + obj.initialize(instID, this.getMetaObject()); + return obj; + } catch (Exception e) { + return null; + } + } + + /** + * Static function to retrieve an instance of the object. + */ + public RelayTuningSettings GetInstance(UAVObjectManager objMngr, long instID) + { + return (RelayTuningSettings)(objMngr.getObject(RelayTuningSettings.OBJID, instID)); + } + + // Constants + protected static final long OBJID = 0xEA358166l; + protected static final String NAME = "RelayTuningSettings"; + protected static String DESCRIPTION = "Setting to run a relay tuning algorithm"; + protected static final boolean ISSINGLEINST = 1 > 0; + protected static final boolean ISSETTINGS = 1 > 0; + protected static int NUMBYTES = 0; + + +} diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/StabilizationDesired.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/StabilizationDesired.java index 68cb82692..31ca2e901 100644 --- a/androidgcs/src/org/openpilot/uavtalk/uavobjects/StabilizationDesired.java +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/StabilizationDesired.java @@ -78,6 +78,8 @@ public class StabilizationDesired extends UAVDataObject { StabilizationModeEnumOptions.add("AxisLock"); StabilizationModeEnumOptions.add("WeakLeveling"); StabilizationModeEnumOptions.add("VirtualBar"); + StabilizationModeEnumOptions.add("RelayRate"); + StabilizationModeEnumOptions.add("RelayAttitude"); fields.add( new UAVObjectField("StabilizationMode", "", UAVObjectField.FieldType.ENUM, StabilizationModeElemNames, StabilizationModeEnumOptions) ); @@ -152,7 +154,7 @@ public class StabilizationDesired extends UAVDataObject { } // Constants - protected static final long OBJID = 0xDE1EAAD6l; + protected static final long OBJID = 0x4FDBFEEAl; protected static final String NAME = "StabilizationDesired"; protected static String DESCRIPTION = "The desired attitude that @ref StabilizationModule will try and achieve if FlightMode is Stabilized. Comes from @ref ManaulControlModule."; protected static final boolean ISSINGLEINST = 1 > 0; diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/StabilizationSettings.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/StabilizationSettings.java index c1270df59..99cfcd830 100644 --- a/androidgcs/src/org/openpilot/uavtalk/uavobjects/StabilizationSettings.java +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/StabilizationSettings.java @@ -131,6 +131,10 @@ public class StabilizationSettings extends UAVDataObject { GyroTauElemNames.add("0"); fields.add( new UAVObjectField("GyroTau", "", UAVObjectField.FieldType.FLOAT32, GyroTauElemNames, null) ); + List DerivativeGammaElemNames = new ArrayList(); + DerivativeGammaElemNames.add("0"); + fields.add( new UAVObjectField("DerivativeGamma", "", UAVObjectField.FieldType.FLOAT32, DerivativeGammaElemNames, null) ); + List WeakLevelingKpElemNames = new ArrayList(); WeakLevelingKpElemNames.add("0"); fields.add( new UAVObjectField("WeakLevelingKp", "(deg/s)/deg", UAVObjectField.FieldType.FLOAT32, WeakLevelingKpElemNames, null) ); @@ -162,6 +166,10 @@ public class StabilizationSettings extends UAVDataObject { VbarMaxAngleElemNames.add("0"); fields.add( new UAVObjectField("VbarMaxAngle", "deg", UAVObjectField.FieldType.UINT8, VbarMaxAngleElemNames, null) ); + List DerivativeCutoffElemNames = new ArrayList(); + DerivativeCutoffElemNames.add("0"); + fields.add( new UAVObjectField("DerivativeCutoff", "Hz", UAVObjectField.FieldType.UINT8, DerivativeCutoffElemNames, null) ); + List MaxAxisLockElemNames = new ArrayList(); MaxAxisLockElemNames.add("0"); fields.add( new UAVObjectField("MaxAxisLock", "deg", UAVObjectField.FieldType.UINT8, MaxAxisLockElemNames, null) ); @@ -263,6 +271,7 @@ public class StabilizationSettings extends UAVDataObject { getField("VbarYawPI").setValue(0.002,1); getField("VbarTau").setValue(0.5); getField("GyroTau").setValue(0.005); + getField("DerivativeGamma").setValue(1); getField("WeakLevelingKp").setValue(0.1); getField("RollMax").setValue(55); getField("PitchMax").setValue(55); @@ -270,6 +279,7 @@ public class StabilizationSettings extends UAVDataObject { getField("VbarGyroSuppress").setValue(30); getField("VbarPiroComp").setValue("FALSE"); getField("VbarMaxAngle").setValue(10); + getField("DerivativeCutoff").setValue(20); getField("MaxAxisLock").setValue(15); getField("MaxAxisLockRate").setValue(2); getField("MaxWeakLevelingRate").setValue(5); @@ -302,7 +312,7 @@ public class StabilizationSettings extends UAVDataObject { } // Constants - protected static final long OBJID = 0xBBC337D4l; + protected static final long OBJID = 0x3D03DE86l; protected static final String NAME = "StabilizationSettings"; protected static String DESCRIPTION = "PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired"; protected static final boolean ISSINGLEINST = 1 > 0; diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/SystemAlarms.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/SystemAlarms.java index 97505641e..bb5d098a4 100644 --- a/androidgcs/src/org/openpilot/uavtalk/uavobjects/SystemAlarms.java +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/SystemAlarms.java @@ -56,7 +56,6 @@ public class SystemAlarms extends UAVDataObject { AlarmElemNames.add("StackOverflow"); AlarmElemNames.add("CPUOverload"); AlarmElemNames.add("EventSystem"); - AlarmElemNames.add("SDCard"); AlarmElemNames.add("Telemetry"); AlarmElemNames.add("ManualControl"); AlarmElemNames.add("Actuator"); @@ -64,7 +63,6 @@ public class SystemAlarms extends UAVDataObject { AlarmElemNames.add("Sensors"); AlarmElemNames.add("Stabilization"); AlarmElemNames.add("Guidance"); - AlarmElemNames.add("AHRSComms"); AlarmElemNames.add("Battery"); AlarmElemNames.add("FlightTime"); AlarmElemNames.add("I2C"); @@ -138,8 +136,6 @@ public class SystemAlarms extends UAVDataObject { getField("Alarm").setValue("Uninitialised",13); getField("Alarm").setValue("Uninitialised",14); getField("Alarm").setValue("Uninitialised",15); - getField("Alarm").setValue("Uninitialised",16); - getField("Alarm").setValue("Uninitialised",17); } @@ -168,7 +164,7 @@ public class SystemAlarms extends UAVDataObject { } // Constants - protected static final long OBJID = 0x9C7CBFEl; + protected static final long OBJID = 0x7BD9C77Al; protected static final String NAME = "SystemAlarms"; protected static String DESCRIPTION = "Alarms from OpenPilot to indicate failure conditions or warnings. Set by various modules."; protected static final boolean ISSINGLEINST = 1 > 0; diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/TaskInfo.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/TaskInfo.java index f9baf33de..8a62efc66 100644 --- a/androidgcs/src/org/openpilot/uavtalk/uavobjects/TaskInfo.java +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/TaskInfo.java @@ -69,6 +69,8 @@ public class TaskInfo extends UAVDataObject { StackRemainingElemNames.add("Com2UsbBridge"); StackRemainingElemNames.add("Usb2ComBridge"); StackRemainingElemNames.add("OveroSync"); + StackRemainingElemNames.add("Autotune"); + StackRemainingElemNames.add("EventDispatcher"); fields.add( new UAVObjectField("StackRemaining", "bytes", UAVObjectField.FieldType.UINT16, StackRemainingElemNames, null) ); List RunningElemNames = new ArrayList(); @@ -89,6 +91,8 @@ public class TaskInfo extends UAVDataObject { RunningElemNames.add("Com2UsbBridge"); RunningElemNames.add("Usb2ComBridge"); RunningElemNames.add("OveroSync"); + RunningElemNames.add("Autotune"); + RunningElemNames.add("EventDispatcher"); List RunningEnumOptions = new ArrayList(); RunningEnumOptions.add("False"); RunningEnumOptions.add("True"); @@ -112,6 +116,8 @@ public class TaskInfo extends UAVDataObject { RunningTimeElemNames.add("Com2UsbBridge"); RunningTimeElemNames.add("Usb2ComBridge"); RunningTimeElemNames.add("OveroSync"); + RunningTimeElemNames.add("Autotune"); + RunningTimeElemNames.add("EventDispatcher"); fields.add( new UAVObjectField("RunningTime", "%", UAVObjectField.FieldType.UINT8, RunningTimeElemNames, null) ); @@ -186,7 +192,7 @@ public class TaskInfo extends UAVDataObject { } // Constants - protected static final long OBJID = 0x51172B8Al; + protected static final long OBJID = 0xB81CD2AEl; protected static final String NAME = "TaskInfo"; protected static String DESCRIPTION = "Task information"; protected static final boolean ISSINGLEINST = 1 > 0; diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/UAVObjectsInitialize.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/UAVObjectsInitialize.java index 88300d54e..f8bd73662 100644 --- a/androidgcs/src/org/openpilot/uavtalk/uavobjects/UAVObjectsInitialize.java +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/UAVObjectsInitialize.java @@ -63,6 +63,7 @@ public class UAVObjectsInitialize { objMngr.registerObject( new GPSSettings() ); objMngr.registerObject( new GPSTime() ); objMngr.registerObject( new GPSVelocity() ); + objMngr.registerObject( new GuidanceSettings() ); objMngr.registerObject( new Gyros() ); objMngr.registerObject( new GyrosBias() ); objMngr.registerObject( new HomeLocation() ); @@ -76,11 +77,15 @@ public class UAVObjectsInitialize { objMngr.registerObject( new NedAccel() ); objMngr.registerObject( new ObjectPersistence() ); objMngr.registerObject( new OveroSyncStats() ); + objMngr.registerObject( new PathAction() ); objMngr.registerObject( new PipXSettings() ); objMngr.registerObject( new PipXStatus() ); objMngr.registerObject( new PositionActual() ); + objMngr.registerObject( new PositionDesired() ); objMngr.registerObject( new RateDesired() ); objMngr.registerObject( new ReceiverActivity() ); + objMngr.registerObject( new RelayTuning() ); + objMngr.registerObject( new RelayTuningSettings() ); objMngr.registerObject( new RevoCalibration() ); objMngr.registerObject( new SonarAltitude() ); objMngr.registerObject( new StabilizationDesired() ); @@ -93,6 +98,8 @@ public class UAVObjectsInitialize { objMngr.registerObject( new VelocityActual() ); objMngr.registerObject( new VelocityDesired() ); objMngr.registerObject( new WatchdogStatus() ); + objMngr.registerObject( new Waypoint() ); + objMngr.registerObject( new WaypointActive() ); } catch (Exception e) { e.printStackTrace(); diff --git a/androidgcs/src/org/openpilot/uavtalk/uavobjects/Waypoint.java b/androidgcs/src/org/openpilot/uavtalk/uavobjects/Waypoint.java index 97dab8a97..017b274f5 100644 --- a/androidgcs/src/org/openpilot/uavtalk/uavobjects/Waypoint.java +++ b/androidgcs/src/org/openpilot/uavtalk/uavobjects/Waypoint.java @@ -58,23 +58,12 @@ public class Waypoint extends UAVDataObject { fields.add( new UAVObjectField("Position", "m", UAVObjectField.FieldType.FLOAT32, PositionElemNames, null) ); List VelocityElemNames = new ArrayList(); - VelocityElemNames.add("North"); - VelocityElemNames.add("East"); - VelocityElemNames.add("Down"); + VelocityElemNames.add("0"); fields.add( new UAVObjectField("Velocity", "m/s", UAVObjectField.FieldType.FLOAT32, VelocityElemNames, null) ); - List YawDesiredElemNames = new ArrayList(); - YawDesiredElemNames.add("0"); - fields.add( new UAVObjectField("YawDesired", "deg", UAVObjectField.FieldType.FLOAT32, YawDesiredElemNames, null) ); - List ActionElemNames = new ArrayList(); ActionElemNames.add("0"); - List ActionEnumOptions = new ArrayList(); - ActionEnumOptions.add("PathToNext"); - ActionEnumOptions.add("EndpointToNext"); - ActionEnumOptions.add("Land"); - ActionEnumOptions.add("Stop"); - fields.add( new UAVObjectField("Action", "", UAVObjectField.FieldType.ENUM, ActionElemNames, ActionEnumOptions) ); + fields.add( new UAVObjectField("Action", "", UAVObjectField.FieldType.UINT8, ActionElemNames, null) ); // Compute the number of bytes for this object @@ -148,7 +137,7 @@ public class Waypoint extends UAVDataObject { } // Constants - protected static final long OBJID = 0x338C5F90l; + protected static final long OBJID = 0xD23852DCl; protected static final String NAME = "Waypoint"; protected static String DESCRIPTION = "A waypoint the aircraft can try and hit. Used by the @ref PathPlanner module"; protected static final boolean ISSINGLEINST = 0 > 0; diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index f786df65c..8687defef 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -68,6 +68,7 @@ USE_GPS ?= YES USE_TXPID ?= YES USE_I2C ?= NO USE_ALTITUDE ?= NO +USE_AUTOTUNE ?= YES TEST_FAULTS ?= NO # List of optional modules to include @@ -94,6 +95,9 @@ endif ifeq ($(TEST_FAULTS), YES) OPTMODULES += Fault endif +ifeq ($(USE_AUTOTUNE), YES) +OPTMODULES += Autotune +endif # List of mandatory modules to include MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP @@ -111,6 +115,8 @@ OPTESTS = ./Tests OPMODULEDIR = ../Modules FLIGHTLIB = ../Libraries FLIGHTLIBINC = $(FLIGHTLIB)/inc +MATHLIB = ../Libraries/math +MATHLIBINC = ../Libraries/math PIOS = ../PiOS PIOSINC = $(PIOS)/inc PIOSSTM32F10X = $(PIOS)/STM32F10x @@ -204,6 +210,8 @@ SRC += $(OPUAVSYNTHDIR)/gpssettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c SRC += $(OPUAVSYNTHDIR)/receiveractivity.c +SRC += $(OPUAVSYNTHDIR)/relaytuningsettings.c +SRC += $(OPUAVSYNTHDIR)/relaytuning.c SRC += $(OPUAVSYNTHDIR)/taskinfo.c SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/ratedesired.c @@ -264,6 +272,8 @@ SRC += $(PIOSCOMMON)/printf-stdarg.c SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/CoordinateConversions.c SRC += $(FLIGHTLIB)/taskmonitor.c +SRC += $(MATHLIB)/sin_lookup.c +SRC += $(MATHLIB)/pid.c ## CMSIS for STM32 SRC += $(CMSISDIR)/core_cm3.c @@ -364,6 +374,7 @@ EXTRAINCDIRS += $(OPUAVSYNTHDIR) EXTRAINCDIRS += $(PIOS) EXTRAINCDIRS += $(PIOSINC) EXTRAINCDIRS += $(FLIGHTLIBINC) +EXTRAINCDIRS += $(MATHLIBINC) EXTRAINCDIRS += $(PIOSSTM32F10X) EXTRAINCDIRS += $(PIOSCOMMON) EXTRAINCDIRS += $(PIOSBOARDS) diff --git a/flight/Libraries/math/pid.c b/flight/Libraries/math/pid.c new file mode 100644 index 000000000..1df416123 --- /dev/null +++ b/flight/Libraries/math/pid.c @@ -0,0 +1,158 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Sine and cosine methods that use a cached lookup table + * @{ + * + * @file pid.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Methods to work with PID structure + * + * @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 + */ + +#include "openpilot.h" +#include "pid.h" + +#define F_PI ((float) M_PI) + +//! Private method +static float bound(float val, float range); + +//! Store the shared time constant for the derivative cutoff. +static float deriv_tau = 7.9577e-3f; + +//! Store the setpoint weight to apply for the derivative term +static float deriv_gamma = 1.0; + +/** + * Update the PID computation + * @param[in] pid The PID struture which stores temporary information + * @param[in] err The error term + * @param[in] dT The time step + * @returns Output the computed controller value + */ +float pid_apply(struct pid *pid, const float err, float dT) +{ + // Scale up accumulator by 1000 while computing to avoid losing precision + pid->iAccumulator += err * (pid->i * dT * 1000.0f); + pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); + + // Calculate DT1 term + float diff = (err - pid->lastErr); + float dterm = 0; + pid->lastErr = err; + if(pid->d && dT) + { + dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); + pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) + } // 7.9577e-3 means 20 Hz f_cutoff + + return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); +} + +/** + * Update the PID computation with setpoint weighting on the derivative + * @param[in] pid The PID struture which stores temporary information + * @param[in] setpoint The setpoint to use + * @param[in] measured The measured value of output + * @param[in] dT The time step + * @returns Output the computed controller value + * + * This version of apply uses setpoint weighting for the derivative component so the gain + * on the gyro derivative can be different than the gain on the setpoint derivative + */ +float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT) +{ + float err = setpoint - measured; + + // Scale up accumulator by 1000 while computing to avoid losing precision + pid->iAccumulator += err * (pid->i * dT * 1000.0f); + pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); + + // Calculate DT1 term, + float dterm = 0; + float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr); + pid->lastErr = (deriv_gamma * setpoint - measured); + if(pid->d && dT) + { + dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); + pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) + } // 7.9577e-3 means 20 Hz f_cutoff + + return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); +} + +/** + * Reset a bit + * @param[in] pid The pid to reset + */ +void pid_zero(struct pid *pid) +{ + if (!pid) + return; + + pid->iAccumulator = 0; + pid->lastErr = 0; + pid->lastDer = 0; +} + +/** + * @brief Configure the common terms that alter ther derivative + * @param[in] cutoff The cutoff frequency (in Hz) + * @param[in] gamma The gamma term for setpoint shaping (unsused now) + */ +void pid_configure_derivative(float cutoff, float g) +{ + deriv_tau = 1.0f / (2 * F_PI * cutoff); + deriv_gamma = g; +} + +/** + * Configure the settings for a pid structure + * @param[out] pid The PID structure to configure + * @param[in] p The proportional term + * @param[in] i The integral term + * @param[in] d The derivative term + */ +void pid_configure(struct pid *pid, float p, float i, float d, float iLim) +{ + if (!pid) + return; + + pid->p = p; + pid->i = i; + pid->d = d; + pid->iLim = iLim; +} + +/** + * Bound input value between limits + */ +static float bound(float val, float range) +{ + if(val < -range) { + val = -range; + } else if(val > range) { + val = range; + } + return val; +} + diff --git a/flight/Libraries/math/pid.h b/flight/Libraries/math/pid.h new file mode 100644 index 000000000..602f560bf --- /dev/null +++ b/flight/Libraries/math/pid.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Sine and cosine methods that use a cached lookup table + * @{ + * + * @file pid.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Methods to work with PID structure + * + * @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 PID_H +#define PID_H + +//! +struct pid { + float p; + float i; + float d; + float iLim; + float iAccumulator; + float lastErr; + float lastDer; +}; + +//! Methods to use the pid structures +float pid_apply(struct pid *pid, const float err, float dT); +float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT); +void pid_zero(struct pid *pid); +void pid_configure(struct pid *pid, float p, float i, float d, float iLim); +void pid_configure_derivative(float cutoff, float gamma); + +#endif /* PID_H */ \ No newline at end of file diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c new file mode 100644 index 000000000..1e703b87f --- /dev/null +++ b/flight/Libraries/math/sin_lookup.c @@ -0,0 +1,142 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Sine and cosine methods that use a cached lookup table + * @{ + * + * @file sin_lookup.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Sine lookup table from flash with 1 degree resolution + * + * @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 + */ + +#include "openpilot.h" +#include "math.h" +#include "stdbool.h" +#include "stdint.h" + +#define FLASH_TABLE +#ifdef FLASH_TABLE + /** Version of the code which precomputes the lookup table in flash **/ + + // This is a precomputed sin lookup table over 90 degrees that + const float sin_table[] = { + 0.000000f,0.017452f,0.034899f,0.052336f,0.069756f,0.087156f,0.104528f,0.121869f,0.139173f,0.156434f, + 0.173648f,0.190809f,0.207912f,0.224951f,0.241922f,0.258819f,0.275637f,0.292372f,0.309017f,0.325568f, + 0.342020f,0.358368f,0.374607f,0.390731f,0.406737f,0.422618f,0.438371f,0.453990f,0.469472f,0.484810f, + 0.500000f,0.515038f,0.529919f,0.544639f,0.559193f,0.573576f,0.587785f,0.601815f,0.615661f,0.629320f, + 0.642788f,0.656059f,0.669131f,0.681998f,0.694658f,0.707107f,0.719340f,0.731354f,0.743145f,0.754710f, + 0.766044f,0.777146f,0.788011f,0.798636f,0.809017f,0.819152f,0.829038f,0.838671f,0.848048f,0.857167f, + 0.866025f,0.874620f,0.882948f,0.891007f,0.898794f,0.906308f,0.913545f,0.920505f,0.927184f,0.933580f, + 0.939693f,0.945519f,0.951057f,0.956305f,0.961262f,0.965926f,0.970296f,0.974370f,0.978148f,0.981627f, + 0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f, + 1.000000f,0.999848f,0.999391f,0.998630f,0.997564f,0.996195f,0.994522f,0.992546f,0.990268f,0.987688f, + 0.984808f,0.981627f,0.978148f,0.974370f,0.970296f,0.965926f,0.961262f,0.956305f,0.951057f,0.945519f, + 0.939693f,0.933580f,0.927184f,0.920505f,0.913545f,0.906308f,0.898794f,0.891007f,0.882948f,0.874620f, + 0.866025f,0.857167f,0.848048f,0.838671f,0.829038f,0.819152f,0.809017f,0.798636f,0.788011f,0.777146f, + 0.766044f,0.754710f,0.743145f,0.731354f,0.719340f,0.707107f,0.694658f,0.681998f,0.669131f,0.656059f, + 0.642788f,0.629320f,0.615661f,0.601815f,0.587785f,0.573576f,0.559193f,0.544639f,0.529919f,0.515038f, + 0.500000f,0.484810f,0.469472f,0.453990f,0.438371f,0.422618f,0.406737f,0.390731f,0.374607f,0.358368f, + 0.342020f,0.325568f,0.309017f,0.292372f,0.275637f,0.258819f,0.241922f,0.224951f,0.207912f,0.190809f, + 0.173648f,0.156434f,0.139173f,0.121869f,0.104528f,0.087156f,0.069756f,0.052336f,0.034899f,0.017452f + }; + +int sin_lookup_initalize() +{ + return 0; +} + +#else +/** Version of the code which allocates the lookup table in heap **/ + +const int SIN_RESOLUTION = 180; + +static float *sin_table; +int sin_lookup_initalize() +{ + // Previously initialized + if (sin_table) + return 0; + + sin_table = (float *) pvPortMalloc(sizeof(float) * SIN_RESOLUTION); + if (sin_table == NULL) + return -1; + + for(uint32_t i = 0; i < 180; i++) + sin_table[i] = sinf((float)i * 2 * M_PI / 360.0f); + + return 0; +} + +#endif + +/** + * Use the lookup table to return sine(angle) where angle is in radians + * to save flash this cheats and uses trig functions to only save + * 90 values + * @param[in] angle Angle in degrees + * @returns sin(angle) +*/ +float sin_lookup_deg(float angle) +{ + if (sin_table == NULL) + return 0; + + int i_ang = ((int32_t) angle) % 360; + if (i_ang >= 180) // for 180 to 360 deg + return -sin_table[i_ang - 180]; + else // for 0 to 179 deg + return sin_table[i_ang]; + + return 0; +} + +/** + * Get cos(angle) using the sine lookup table + * @param[in] angle Angle in degrees + * @returns cos(angle) + */ +float cos_lookup_deg(float angle) +{ + return sin_lookup_deg(angle + 90); +} + +/** + * Use the lookup table to return sine(angle) where angle is in radians + * @param[in] angle Angle in radians + * @returns sin(angle) + */ +float sin_lookup_rad(float angle) +{ + int degrees = angle * 180.0f / M_PI; + return sin_lookup_deg(degrees); +} + +/** + * Use the lookup table to return sine(angle) where angle is in radians + * @param[in] angle Angle in radians + * @returns cos(angle) + */ +float cos_lookup_rad(float angle) +{ + int degrees = angle * 180.0f / M_PI; + return cos_lookup_deg(degrees); +} \ No newline at end of file diff --git a/flight/Libraries/math/sin_lookup.h b/flight/Libraries/math/sin_lookup.h new file mode 100644 index 000000000..adc29f66a --- /dev/null +++ b/flight/Libraries/math/sin_lookup.h @@ -0,0 +1,40 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Sine and cosine methods that use a cached lookup table + * @{ + * + * @file sin_lookup.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Sine lookup table from flash with 1 degree resolution + * + * @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 SIN_LOOKUP_H +#define SIN_LOOKUP_H + +int sin_lookup_initalize(); +float sin_lookup_deg(float angle); +float cos_lookup_deg(float angle); +float sin_lookup_rad(float angle); +float cos_lookup_rad(float angle); + +#endif \ No newline at end of file diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c new file mode 100644 index 000000000..96051b076 --- /dev/null +++ b/flight/Modules/Autotune/autotune.c @@ -0,0 +1,333 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Autotuning module + * @brief Reads from @ref ManualControlCommand and fakes a rate mode while + * toggling the channels to relay mode + * @{ + * + * @file autotune.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Module to handle all comms to the AHRS on a periodic basis. + * + * @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 + */ + +/** + * Input objects: None, takes sensor data via pios + * Output objects: @ref AttitudeRaw @ref AttitudeActual + * + * This module computes an attitude estimate from the sensor data + * + * The module executes in its own thread. + * + * 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 "pios.h" +#include "flightstatus.h" +#include "hwsettings.h" +#include "manualcontrolcommand.h" +#include "manualcontrolsettings.h" +#include "relaytuning.h" +#include "relaytuningsettings.h" +#include "stabilizationdesired.h" +#include "stabilizationsettings.h" +#include + +// Private constants +#define STACK_SIZE_BYTES 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY+2) + +// Private types +enum AUTOTUNE_STATE {AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET}; + +// Private variables +static xTaskHandle taskHandle; +static bool autotuneEnabled; + +// Private functions +static void AutotuneTask(void *parameters); +static void update_stabilization_settings(); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t AutotuneInitialize(void) +{ + // Create a queue, connect to manual control command and flightstatus +#ifdef MODULE_AUTOTUNE_BUILTIN + autotuneEnabled = true; +#else + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + + HwSettingsOptionalModulesGet(optionalModules); + + if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED) + autotuneEnabled = true; + else + autotuneEnabled = false; +#endif + + return 0; +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t AutotuneStart(void) +{ + // Start main task if it is enabled + if(autotuneEnabled) { + xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + + TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); + } + return 0; +} + +MODULE_INITCALL(AutotuneInitialize, AutotuneStart) + +/** + * Module thread, should not return. + */ +static void AutotuneTask(void *parameters) +{ + //AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + + enum AUTOTUNE_STATE state = AT_INIT; + + portTickType lastUpdateTime = xTaskGetTickCount(); + + while(1) { + + PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); + // TODO: + // 1. get from queue + // 2. based on whether it is flightstatus or manualcontrol + + portTickType diffTime; + + const uint32_t PREPARE_TIME = 2000; + const uint32_t MEAURE_TIME = 30000; + + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + // Only allow this module to run when autotuning + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { + state = AT_INIT; + vTaskDelay(50); + continue; + } + + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); + + ManualControlSettingsData manualSettings; + ManualControlSettingsGet(&manualSettings); + + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); + + bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE; + + if (rate) { // rate mode + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + + stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; + stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; + } else { + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + + stabDesired.Roll = manualControl.Roll * stabSettings.RollMax; + stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax; + } + + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + stabDesired.Throttle = manualControl.Throttle; + + switch(state) { + case AT_INIT: + + lastUpdateTime = xTaskGetTickCount(); + + // Only start when armed and flying + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) + state = AT_START; + break; + + case AT_START: + + diffTime = xTaskGetTickCount() - lastUpdateTime; + + // Spend the first block of time in normal rate mode to get airborne + if (diffTime > PREPARE_TIME) { + state = AT_ROLL; + lastUpdateTime = xTaskGetTickCount(); + } + break; + + case AT_ROLL: + + diffTime = xTaskGetTickCount() - lastUpdateTime; + + // Run relay mode on the roll axis for the measurement time + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : + STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; + if (diffTime > MEAURE_TIME) { // Move on to next state + state = AT_PITCH; + lastUpdateTime = xTaskGetTickCount(); + } + break; + + case AT_PITCH: + + diffTime = xTaskGetTickCount() - lastUpdateTime; + + // Run relay mode on the pitch axis for the measurement time + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : + STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; + if (diffTime > MEAURE_TIME) { // Move on to next state + state = AT_FINISHED; + lastUpdateTime = xTaskGetTickCount(); + } + break; + + case AT_FINISHED: + + // Wait until disarmed and landed before updating the settings + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) + state = AT_SET; + + break; + + case AT_SET: + update_stabilization_settings(); + state = AT_INIT; + break; + + default: + // Set an alarm or some shit like that + break; + } + + StabilizationDesiredSet(&stabDesired); + + vTaskDelay(10); + } +} + +/** + * Called after measuring roll and pitch to update the + * stabilization settings + * + * takes in @ref RelayTuning and outputs @ref StabilizationSettings + */ +static void update_stabilization_settings() +{ + RelayTuningData relayTuning; + RelayTuningGet(&relayTuning); + + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); + + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); + + // Eventually get these settings from RelayTuningSettings + const float gain_ratio_r = 1.0f / 3.0f; + const float zero_ratio_r = 1.0f / 3.0f; + const float gain_ratio_p = 1.0f / 5.0f; + const float zero_ratio_p = 1.0f / 5.0f; + + // For now just run over roll and pitch + for (uint i = 0; i < 2; i++) { + float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s) + + float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) + float zc = wc * zero_ratio_r; // controller zero location (rad/s) + float kpu = 4.0f / M_PI / relayTuning.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity + float kp = kpu * gain_ratio_r; // proportional gain + float ki = zc * kp; // integral gain + + // Now calculate gains for the next loop out knowing it is the integral of + // the inner loop -- the plant is position/velocity = scale*1/s + float wc2 = wc * gain_ratio_p; // crossover of the attitude loop + float kp2 = wc2; // kp of attitude + float ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude + + switch(i) { + case 0: // roll + stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; + stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; + stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; + stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; + break; + case 1: // Pitch + stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; + stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; + stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; + stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; + break; + default: + // Oh shit oh shit oh shit + break; + } + } + switch(relaySettings.Behavior) { + case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE: + // Just measure, don't update the stab settings + break; + case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE: + StabilizationSettingsSet(&stabSettings); + break; + case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE: + StabilizationSettingsSet(&stabSettings); + UAVObjSave(StabilizationSettingsHandle(), 0); + break; + } + +} + +/** + * @} + * @} + */ diff --git a/flight/Modules/Autotune/inc/autotune.h b/flight/Modules/Autotune/inc/autotune.h new file mode 100644 index 000000000..3f90e56a6 --- /dev/null +++ b/flight/Modules/Autotune/inc/autotune.h @@ -0,0 +1,37 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Attitude Attitude Module + * @{ + * + * @file attitude.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief Acquires sensor data and fuses it into attitude estimate for CC + * + * @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 ATTITUDE_H +#define ATTITUDE_H + +#include "openpilot.h" + +int32_t AttitudeInitialize(void); + +#endif // ATTITUDE_H diff --git a/flight/Modules/ManualControl/inc/manualcontrol.h b/flight/Modules/ManualControl/inc/manualcontrol.h index f71cdebde..b67366f92 100644 --- a/flight/Modules/ManualControl/inc/manualcontrol.h +++ b/flight/Modules/ManualControl/inc/manualcontrol.h @@ -32,7 +32,7 @@ #include "manualcontrolcommand.h" -typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3} flightmode_path; +typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4} flightmode_path; #define PARSE_FLIGHT_MODE(x) ( \ (x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \ @@ -41,7 +41,8 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \ (x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \ (x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \ + (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : FLIGHTMODE_UNDEFINED \ ) int32_t ManualControlInitialize(); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 7d0e78881..2c7a68575 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -401,6 +401,10 @@ static void manualControlTask(void *parameters) case FLIGHTMODE_STABILIZED: updateStabilizationDesired(&cmd, &settings); break; + case FLIGHTMODE_TUNING: + // Tuning takes settings directly from manualcontrolcommand. No need to + // call anything else. This just avoids errors. + break; case FLIGHTMODE_GUIDANCE: switch(flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: @@ -602,6 +606,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (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 : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : @@ -610,6 +616,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (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 : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : @@ -618,6 +626,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (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 : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; diff --git a/flight/Modules/Stabilization/inc/relay_tuning.h b/flight/Modules/Stabilization/inc/relay_tuning.h new file mode 100644 index 000000000..9044986c5 --- /dev/null +++ b/flight/Modules/Stabilization/inc/relay_tuning.h @@ -0,0 +1,39 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Relay tuning controller + * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" + * @{ + * + * @file relay_tuning.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Attitude stabilization module. + * + * @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 RELAY_TUNING_H +#define RELAY_TUNING_H + +int stabilization_relay_rate(float err, float *output, int axis, bool reinit); + +#endif \ No newline at end of file diff --git a/flight/Modules/Stabilization/inc/stabilization.h b/flight/Modules/Stabilization/inc/stabilization.h index 89a816c24..ec0ae4e9f 100644 --- a/flight/Modules/Stabilization/inc/stabilization.h +++ b/flight/Modules/Stabilization/inc/stabilization.h @@ -33,6 +33,8 @@ #ifndef STABILIZATION_H #define STABILIZATION_H +enum {ROLL,PITCH,YAW,MAX_AXES}; + int32_t StabilizationInitialize(); #endif // STABILIZATION_H diff --git a/flight/Modules/Stabilization/inc/virtualflybar.h b/flight/Modules/Stabilization/inc/virtualflybar.h new file mode 100644 index 000000000..8ef54ce52 --- /dev/null +++ b/flight/Modules/Stabilization/inc/virtualflybar.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Virtual flybar mode + * @note This file implements the logic for a virtual flybar + * @{ + * + * @file virtualflybar.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Attitude stabilization module. + * + * @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 VIRTUALFLYBAR_H + #define VIRTUALFLYBAR_H + +#include "openpilot.h" +#include "stabilizationsettings.h" + +int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings); +int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT); + + #endif /* VIRTUALFLYBAR_H */ \ No newline at end of file diff --git a/flight/Modules/Stabilization/relay_tuning.c b/flight/Modules/Stabilization/relay_tuning.c new file mode 100644 index 000000000..5d53aa7d3 --- /dev/null +++ b/flight/Modules/Stabilization/relay_tuning.c @@ -0,0 +1,151 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Relay tuning controller + * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" + * @{ + * + * @file stabilization.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Attitude stabilization module. + * + * @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 + */ + +#include "openpilot.h" +#include "relaytuning.h" +#include "relaytuningsettings.h" +#include "sin_lookup.h" + +//! Private variables +static const int SIN_RESOLUTION = 180; + +#define MAX_AXES 3 + +/** + * Apply a step function for the stabilization controller and monitor the + * result + * + * Used to Replace the rate PID with a relay to measure the critical properties of this axis + * i.e. period and gain + */ +int stabilization_relay_rate(float error, float *output, int axis, bool reinit) +{ + RelayTuningData relay; + RelayTuningGet(&relay); + + static portTickType lastHighTime; + static portTickType lastLowTime; + + static float accum_sin, accum_cos; + static uint32_t accumulated = 0; + + const uint16_t DEGLITCH_TIME = 20; // ms + const float AMPLITUDE_ALPHA = 0.95; + const float PERIOD_ALPHA = 0.95; + + portTickType thisTime = xTaskGetTickCount(); + + static bool rateRelayRunning[MAX_AXES]; + + // This indicates the current estimate of the smoothed error. So when it is high + // we are waiting for it to go low. + static bool high = false; + + // On first run initialize estimates to something reasonable + if(reinit) { + rateRelayRunning[axis] = false; + relay.Period[axis] = 200; + relay.Gain[axis] = 0; + + accum_sin = 0; + accum_cos = 0; + accumulated = 0; + + // These should get reinitialized anyway + high = true; + lastHighTime = thisTime; + lastLowTime = thisTime; + RelayTuningSet(&relay); + } + + + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); + + // Compute output, simple threshold on error + *output = high ? relaySettings.Amplitude : -relaySettings.Amplitude; + + /**** The code below here is to estimate the properties of the oscillation ****/ + + // Make sure the period can't go below limit + if (relay.Period[axis] < DEGLITCH_TIME) + relay.Period[axis] = DEGLITCH_TIME; + + // Project the error onto a sine and cosine of the same frequency + // to accumulate the average amplitude + int32_t dT = thisTime - lastHighTime; + float phase = ((float)360 * (float)dT) / relay.Period[axis]; + if(phase >= 360) + phase = 0; + accum_sin += sin_lookup_deg(phase) * error; + accum_cos += sin_lookup_deg(phase + 90) * error; + accumulated ++; + + // Make sure we've had enough time since last transition then check for a change in the output + bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME; + + if ( !high && time_hysteresis && error > relaySettings.HysteresisThresh ){ + /* POSITIVE CROSSING DETECTED */ + + float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated; + float this_gain = this_amplitude / relaySettings.Amplitude; + + accumulated = 0; + accum_sin = 0; + accum_cos = 0; + + if(rateRelayRunning[axis] == false) { + rateRelayRunning[axis] = true; + relay.Period[axis] = 200; + relay.Gain[axis] = 0; + } else { + // Low pass filter each amplitude and period + relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); + relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); + } + lastHighTime = thisTime; + high = true; + RelayTuningSet(&relay); + + } else if ( high && time_hysteresis && error < -relaySettings.HysteresisThresh ) { + /* FALLING CROSSING DETECTED */ + + lastLowTime = thisTime; + high = false; + + } + + return 0; +} + + diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 43bb9b0d5..fc4a5dcc5 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -36,12 +36,22 @@ #include "stabilizationsettings.h" #include "actuatordesired.h" #include "ratedesired.h" +#include "relaytuning.h" +#include "relaytuningsettings.h" #include "stabilizationdesired.h" #include "attitudeactual.h" #include "gyros.h" #include "flightstatus.h" #include "manualcontrol.h" // Just to get a macro + +// Math libraries #include "CoordinateConversions.h" +#include "pid.h" +#include "sin_lookup.h" + +// Includes for various stabilization algorithms +#include "relay_tuning.h" +#include "virtualflybar.h" // Private constants #define MAX_QUEUE_SIZE 1 @@ -55,44 +65,26 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY+4) #define FAILSAFE_TIMEOUT_MS 30 -enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_VBAR_ROLL, PID_VBAR_PITCH, PID_VBAR_YAW, PID_MAX}; +enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX}; -enum {ROLL,PITCH,YAW,MAX_AXES}; - - -// Private types -typedef struct { - float p; - float i; - float d; - float iLim; - float iAccumulator; - float lastErr; -} pid_type; // Private variables static xTaskHandle taskHandle; static StabilizationSettingsData settings; 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; uint8_t weak_leveling_max = 0; bool lowThrottleZeroIntegral; -float vbar_integral[3] = {0, 0, 0}; float vbar_decay = 0.991f; -pid_type pids[PID_MAX]; -int8_t vbar_gyros_suppress; -bool vbar_piro_comp = false; +struct pid pids[PID_MAX]; // Private functions static void stabilizationTask(void* parameters); -static float ApplyPid(pid_type * pid, const float err, float dT); -static float bound(float val); +static float bound(float val, float range); static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent * ev); @@ -108,10 +100,10 @@ int32_t StabilizationStart() // Listen for updates. // AttitudeActualConnectQueue(queue); GyrosConnectQueue(queue); - + StabilizationSettingsConnectCallback(SettingsUpdatedCb); SettingsUpdatedCb(StabilizationSettingsHandle()); - + // Start main task xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle); @@ -131,6 +123,11 @@ int32_t StabilizationInitialize() RateDesiredInitialize(); #endif + // Code required for relay tuning + sin_lookup_initalize(); + RelayTuningSettingsInitialize(); + RelayTuningInitialize(); + return 0; } @@ -142,73 +139,73 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart) static void stabilizationTask(void* parameters) { UAVObjEvent ev; - + uint32_t timeval = PIOS_DELAY_GetRaw(); - + ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; RateDesiredData rateDesired; AttitudeActualData attitudeActual; GyrosData gyrosData; FlightStatusData flightStatus; - + SettingsUpdatedCb((UAVObjEvent *) NULL); - + // Main task loop ZeroPids(); while(1) { float dT; - + PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); - + // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); continue; } - + dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; timeval = PIOS_DELAY_GetRaw(); - + FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); GyrosGet(&gyrosData); - + #if defined(DIAGNOSTICS) RateDesiredGet(&rateDesired); #endif - + #if defined(PIOS_QUATERNION_STABILIZATION) // Quaternion calculation of error in each axis. Uses more memory. float rpy_desired[3]; float q_desired[4]; float q_error[4]; float local_error[3]; - + // Essentially zero errors for anything in rate or none if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[0] = stabDesired.Roll; else rpy_desired[0] = attitudeActual.Roll; - + if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[1] = stabDesired.Pitch; else rpy_desired[1] = attitudeActual.Pitch; - + if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[2] = stabDesired.Yaw; else rpy_desired[2] = attitudeActual.Yaw; - + RPY2Quaternion(rpy_desired, q_desired); quat_inverse(q_desired); quat_mult(q_desired, &attitudeActual.q1, q_error); quat_inverse(q_error); Quaternion2RPY(q_error, local_error); - + #else // Simpler algorithm for CC, less memory float local_error[3] = {stabDesired.Roll - attitudeActual.Roll, @@ -217,7 +214,7 @@ static void stabilizationTask(void* parameters) local_error[2] = fmodf(local_error[2] + 180, 360) - 180; #endif - + float gyro_filtered[3]; gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha); gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha); @@ -226,49 +223,79 @@ static void stabilizationTask(void* parameters) float *actuatorDesiredAxis = &actuatorDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; - //Calculate desired rate + ActuatorDesiredGet(&actuatorDesired); + + // A flag to track which stabilization mode each axis is in + static uint8_t previous_mode[MAX_AXES] = {255,255,255}; + bool error = false; + + //Run the selected stabilization algorithm on each axis: for(uint8_t i=0; i< MAX_AXES; i++) { + // Check whether this axis mode needs to be reinitialized + bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); + previous_mode[i] = stabDesired.StabilizationMode[i]; + + // Apply the selected control law switch(stabDesired.StabilizationMode[i]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: + if(reinit) + pids[PID_RATE_ROLL + i].iAccumulator = 0; + + // Store to rate desired variable for storing to UAVO + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + + // Compute the inner loop + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); + + break; + + case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: + if(reinit) { + pids[PID_ROLL + i].iAccumulator = 0; + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } + + // Compute the outer loop + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + + // Compute the inner loop + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); + + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: + + // Store for debugging output rateDesiredAxis[i] = attitudeDesiredAxis[i]; - // Zero attitude and axis lock accumulators - pids[PID_ROLL + i].iAccumulator = 0; - axis_lock_accum[i] = 0; - break; + // Run a virtual flybar stabilization algorithm on this axis + stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings); + break; case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { + if (reinit) + pids[PID_RATE_ROLL + i].iAccumulator = 0; + float weak_leveling = local_error[i] * weak_leveling_kp; + weak_leveling = bound(weak_leveling, weak_leveling_max); - if(weak_leveling > weak_leveling_max) - weak_leveling = weak_leveling_max; - if(weak_leveling < -weak_leveling_max) - weak_leveling = -weak_leveling_max; - + // Compute desired rate as input biased towards leveling rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); - // Zero attitude and axis lock accumulators - pids[PID_ROLL + i].iAccumulator = 0; - axis_lock_accum[i] = 0; break; } - case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: - rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT); - - if(rateDesiredAxis[i] > settings.MaximumRate[i]) - rateDesiredAxis[i] = settings.MaximumRate[i]; - else if(rateDesiredAxis[i] < -settings.MaximumRate[i]) - rateDesiredAxis[i] = -settings.MaximumRate[i]; - - - axis_lock_accum[i] = 0; - break; - case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: + if (reinit) + pids[PID_RATE_ROLL + i].iAccumulator = 0; + if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) { // While getting strong commands act like rate mode rateDesiredAxis[i] = attitudeDesiredAxis[i]; @@ -276,154 +303,96 @@ static void stabilizationTask(void* parameters) } else { // For weaker commands or no command simply attitude lock (almost) on no gyro change axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; - if(axis_lock_accum[i] > max_axis_lock) - axis_lock_accum[i] = max_axis_lock; - else if(axis_lock_accum[i] < -max_axis_lock) - axis_lock_accum[i] = -max_axis_lock; - - rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT); + axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock); + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); } - - if(rateDesiredAxis[i] > settings.MaximumRate[i]) - rateDesiredAxis[i] = settings.MaximumRate[i]; - else if(rateDesiredAxis[i] < -settings.MaximumRate[i]) - rateDesiredAxis[i] = -settings.MaximumRate[i]; + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); + + break; + + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: + // Store to rate desired variable for storing to UAVO + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + + // Run the relay controller which also estimates the oscillation parameters + stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0); + + break; + + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE: + if(reinit) + pids[PID_ROLL + i].iAccumulator = 0; + + // Compute the outer loop like attitude mode + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + + // Run the relay controller which also estimates the oscillation parameters + stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0); + + break; + + case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: + actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i],1.0f); + break; + default: + error = true; break; } } - // Piro compensation rotates the virtual flybar by yaw step to keep it - // rotated to external world - if (vbar_piro_comp) { - const float F_PI = 3.14159f; - float cy = cosf(gyro_filtered[2] / 180.0f * F_PI * dT); - float sy = sinf(gyro_filtered[2] / 180.0f * F_PI * dT); - - float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0]; - float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0]; - - vbar_integral[1] = vbar_pitch; - vbar_integral[0] = vbar_roll; - } + if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) + stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); - uint8_t shouldUpdate = 1; #if defined(DIAGNOSTICS) RateDesiredSet(&rateDesired); #endif - ActuatorDesiredGet(&actuatorDesired); - //Calculate desired command - for(uint32_t i = 0; i < MAX_AXES; i++) - { - switch(stabDesired.StabilizationMode[i]) - { - case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: - case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: - case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: - case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: - { - float command = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(command); - break; - } - case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: - { - // Track the angle of the virtual flybar which includes a slow decay - vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT; - if (vbar_integral[i] > settings.VbarMaxAngle) - vbar_integral[i] = settings.VbarMaxAngle; - if (-vbar_integral[i] < -settings.VbarMaxAngle) - vbar_integral[i] = -settings.VbarMaxAngle; - - // Command signal is composed of stick input added to the gyro and virtual flybar - float gyro_gain = 1.0f; - if (vbar_gyros_suppress > 0) { - gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f); - gyro_gain = (gyro_gain < 0) ? 0 : 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); - - actuatorDesiredAxis[i] = bound(command); - break; - } - case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: - switch (i) - { - case ROLL: - actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]); - shouldUpdate = 1; - pids[PID_RATE_ROLL].iAccumulator = 0; - pids[PID_ROLL].iAccumulator = 0; - break; - case PITCH: - actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]); - shouldUpdate = 1; - pids[PID_RATE_PITCH].iAccumulator = 0; - pids[PID_PITCH].iAccumulator = 0; - break; - case YAW: - actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]); - shouldUpdate = 1; - pids[PID_RATE_YAW].iAccumulator = 0; - pids[PID_YAW].iAccumulator = 0; - break; - } - break; - - } - } // Save dT actuatorDesired.UpdateTime = dT * 1000; + actuatorDesired.Throttle = stabDesired.Throttle; - if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL) - shouldUpdate = 0; - - if(shouldUpdate) - { - actuatorDesired.Throttle = stabDesired.Throttle; - if(dT > 15) - actuatorDesired.NumLongUpdates++; + if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) { ActuatorDesiredSet(&actuatorDesired); + } else { + // Force all axes to reinitialize when engaged + for(uint8_t i=0; i< MAX_AXES; i++) + previous_mode[i] = 255; } if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || - (lowThrottleZeroIntegral && stabDesired.Throttle < 0) || - !shouldUpdate) + (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) { - ZeroPids(); + // Force all axes to reinitialize when engaged + for(uint8_t i=0; i< MAX_AXES; i++) + previous_mode[i] = 255; } - - // Clear alarms - AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); + // Clear or set alarms. Done like this to prevent toggline each cycle + // and hammering system alarms + if (error) + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR); + else + AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } } -float ApplyPid(pid_type * pid, const float err, float dT) -{ - float diff = (err - pid->lastErr); - pid->lastErr = err; - - // Scale up accumulator by 1000 while computing to avoid losing precision - pid->iAccumulator += err * (pid->i * dT * 1000.0f); - if(pid->iAccumulator > (pid->iLim * 1000.0f)) { - pid->iAccumulator = pid->iLim * 1000.0f; - } else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) { - pid->iAccumulator = -pid->iLim * 1000.0f; - } - return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT)); -} - +/** + * Clear the accumulators and derivatives for all the axes + */ static void ZeroPids(void) { - for(int8_t ct = 0; ct < PID_MAX; ct++) { - pids[ct].iAccumulator = 0.0f; - pids[ct].lastErr = 0.0f; - } + for(uint32_t i = 0; i < PID_MAX; i++) + pid_zero(&pids[i]); + + for(uint8_t i = 0; i < 3; i++) axis_lock_accum[i] = 0.0f; } @@ -432,12 +401,12 @@ static void ZeroPids(void) /** * Bound input value between limits */ -static float bound(float val) +static float bound(float val, float range) { - if(val < -1.0f) { - val = -1.0f; - } else if(val > 1.0f) { - val = 1.0f; + if(val < -range) { + val = -range; + } else if(val > range) { + val = range; } return val; } @@ -445,68 +414,54 @@ static float bound(float val) static void SettingsUpdatedCb(UAVObjEvent * ev) { StabilizationSettingsGet(&settings); - + // Set the roll rate PID constants - pids[PID_RATE_ROLL].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP]; - pids[PID_RATE_ROLL].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI]; - pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD]; - pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]; - + pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], + settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], + pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], + pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]); + // Set the pitch rate PID constants - pids[PID_RATE_PITCH].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP]; - pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI]; - pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD]; - pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]; - + pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], + pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], + pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], + pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]); + // Set the yaw rate PID constants - pids[PID_RATE_YAW].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP]; - pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI]; - pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD]; - pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]; - + pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], + pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], + pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], + pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]); + // Set the roll attitude PI constants - pids[PID_ROLL].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP]; - pids[PID_ROLL].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI]; - pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]; - - // Set the pitch attitude PI constants - pids[PID_PITCH].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP]; - pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI]; - pids[PID_PITCH].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]; - - // Set the yaw attitude PI constants - pids[PID_YAW].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP]; - pids[PID_YAW].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI]; - pids[PID_YAW].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]; - - // Set the roll attitude PI constants - pids[PID_VBAR_ROLL].p = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; - pids[PID_VBAR_ROLL].i = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], + settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0, + pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]); // Set the pitch attitude PI constants - pids[PID_VBAR_PITCH].p = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KP]; - pids[PID_VBAR_PITCH].i = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KI]; + pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], + pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0, + settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]); // Set the yaw attitude PI constants - 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]; + pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], + settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0, + settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]); + + // Set up the derivative term + pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); // Maximum deviation to accumulate for axis lock max_axis_lock = settings.MaxAxisLock; max_axislock_rate = settings.MaxAxisLockRate; - + // Settings for weak leveling weak_leveling_kp = settings.WeakLevelingKp; weak_leveling_max = settings.MaxWeakLevelingRate; - + // Whether to zero the PID integrals while throttle is low lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; - + // The dT has some jitter iteration to iteration that we don't want to // make thie result unpredictable. Still, it's nicer to specify the constant // based on a time (in ms) rather than a fixed multiplier. The error between @@ -517,11 +472,9 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) gyro_alpha = 0; // not trusting this to resolve to 0 else gyro_alpha = expf(-fakeDt / settings.GyroTau); - + // Compute time constant for vbar decay term based on a tau vbar_decay = expf(-fakeDt / settings.VbarTau); - vbar_gyros_suppress = settings.VbarGyroSuppress; - vbar_piro_comp = settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE; } @@ -529,3 +482,4 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) * @} * @} */ + diff --git a/flight/Modules/Stabilization/virtualflybar.c b/flight/Modules/Stabilization/virtualflybar.c new file mode 100644 index 000000000..bbfe69eae --- /dev/null +++ b/flight/Modules/Stabilization/virtualflybar.c @@ -0,0 +1,119 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Virtual flybar mode + * @note This file implements the logic for a virtual flybar + * @{ + * + * @file virtualflybar.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Attitude stabilization module. + * + * @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 + */ + +#include "openpilot.h" +#include "stabilization.h" +#include "stabilizationsettings.h" + +//! Private variables +static float vbar_integral[MAX_AXES]; +static float vbar_decay = 0.991f; + +//! Private methods +static float bound(float val, float range); + +int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings) +{ + float gyro_gain = 1.0f; + float kp = 0, ki = 0; + + if(reinit) + vbar_integral[axis] = 0; + + // Track the angle of the virtual flybar which includes a slow decay + vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT; + vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle); + + // Command signal can indicate how much to disregard the gyro feedback (fast flips) + if (settings->VbarGyroSuppress > 0) { + gyro_gain = (1.0f - fabs(command) * settings->VbarGyroSuppress / 100.0f); + gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; + } + + // Get the settings for the correct axis + switch(axis) + { + case ROLL: + kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + case PITCH: + kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + case YAW: + kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + default: + PIOS_DEBUG_Assert(0); + } + + // Command signal is composed of stick input added to the gyro and virtual flybar + *output = command * settings->VbarSensitivity[axis] - + gyro_gain * (vbar_integral[axis] * ki + gyro * kp); + + return 0; +} + +/** + * Want to keep the virtual flybar fixed in world coordinates as we pirouette + * @param[in] z_gyro The deg/s of rotation along the z axis + * @param[in] dT The time since last sample + */ +int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT) +{ + const float F_PI = (float) M_PI; + float cy = cosf(z_gyro / 180.0f * F_PI * dT); + float sy = sinf(z_gyro / 180.0f * F_PI * dT); + + float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0]; + float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0]; + + vbar_integral[1] = vbar_pitch; + vbar_integral[0] = vbar_roll; + + return 0; +} + +/** + * Bound input value between limits + */ +static float bound(float val, float range) +{ + if(val < -range) { + val = -range; + } else if(val > range) { + val = range; + } + return val; +} diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index 10a7a41cd..85143fc17 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -74,6 +74,7 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 #define PIOS_WDG_STABILIZATION 0x0002 #define PIOS_WDG_ATTITUDE 0x0004 #define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_AUTOTUNE 0x0010 //------------------------ // TELEMETRY diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index 462acfc34..bd0612412 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -62,6 +62,8 @@ UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += positionactual UAVOBJSRCFILENAMES += positiondesired UAVOBJSRCFILENAMES += ratedesired +UAVOBJSRCFILENAMES += relaytuning +UAVOBJSRCFILENAMES += relaytuningsettings UAVOBJSRCFILENAMES += revocalibration UAVOBJSRCFILENAMES += sonaraltitude UAVOBJSRCFILENAMES += stabilizationdesired diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp index f8db6fbc6..9cf05e69c 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp @@ -22,8 +22,7 @@ QextSerialEnumerator::~QextSerialEnumerator( ) // static QList QextSerialEnumerator::getPorts() -{ - QList infoList; +{ QList infoList; io_iterator_t serialPortIterator = 0; kern_return_t kernResult = KERN_FAILURE; CFMutableDictionaryRef matchingDictionary; @@ -44,18 +43,6 @@ QList QextSerialEnumerator::getPorts() IOObjectRelease(serialPortIterator); serialPortIterator = 0; - if( !(matchingDictionary = IOServiceNameMatching("AppleUSBCDC")) ) { - qWarning("IOServiceNameMatching returned a NULL dictionary."); - return infoList; - } - - if( IOServiceGetMatchingServices(kIOMasterPortDefault, matchingDictionary, &serialPortIterator) != KERN_SUCCESS ) { - qCritical() << "IOServiceGetMatchingServices failed, returned" << kernResult; - return infoList; - } - iterateServicesOSX(serialPortIterator, infoList); - IOObjectRelease(serialPortIterator); - return infoList; } diff --git a/ground/openpilotgcs/src/plugins/config/autotune.ui b/ground/openpilotgcs/src/plugins/config/autotune.ui new file mode 100644 index 000000000..113144535 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/autotune.ui @@ -0,0 +1,1084 @@ + + + AutotuneWidget + + + + 0 + 0 + 443 + 575 + + + + Form + + + + + 20 + 10 + 401 + 131 + + + + Tuning Aggressiveness + + + + + + Rate Tuning: + + + + + + + Attitude Tuning: + + + + + + + 100 + + + Qt::Horizontal + + + + objname:RelayTuningSettings + fieldname:RateGain + scale:0.01 + haslimits:no + + + + + + + + 100 + + + Qt::Horizontal + + + + objname:RelayTuningSettings + fieldname:AttitudeGain + scale:0.01 + haslimits:no + + + + + + + + + + 20 + 250 + 401 + 121 + + + + Computed Values + + + + + + RateKp + + + + + + + Roll + + + + + + + RateKi + + + + + + + AttitudeKp + + + + + + + AttitudeKi + + + + + + + Pitch + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + + + 20 + 140 + 401 + 111 + + + + Measured Properties + + + + + + Roll: + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + objname:RelayTuning + fieldname:Period + element:Roll + + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + objname:RelayTuning + fieldname:Gain + element:Roll + + + + + + + + Period (ms) + + + + + + + Gain (deg/s) / output + + + + + + + Pitch + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + objname:RelayTuning + fieldname:Period + element:Pitch + + + + + + + + 0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + objname:RelayTuning + fieldname:Gain + element:Pitch + + + + + + + + + + 20 + 480 + 401 + 79 + + + + + 0 + 0 + + + + + 0 + 79 + + + + + 16777215 + 79 + + + + + + + + + 0 + 0 + 0 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 255 + 255 + 255 + + + + + + + 251 + 251 + 251 + + + + + + + 124 + 124 + 124 + + + + + + + 165 + 165 + 165 + + + + + + + 0 + 0 + 0 + + + + + + + 255 + 255 + 255 + + + + + + + 0 + 0 + 0 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 0 + 0 + 0 + + + + + + + 251 + 251 + 251 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 0 + 0 + 0 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 255 + 255 + 255 + + + + + + + 251 + 251 + 251 + + + + + + + 124 + 124 + 124 + + + + + + + 165 + 165 + 165 + + + + + + + 0 + 0 + 0 + + + + + + + 255 + 255 + 255 + + + + + + + 0 + 0 + 0 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 0 + 0 + 0 + + + + + + + 251 + 251 + 251 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 124 + 124 + 124 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 255 + 255 + 255 + + + + + + + 251 + 251 + 251 + + + + + + + 124 + 124 + 124 + + + + + + + 165 + 165 + 165 + + + + + + + 124 + 124 + 124 + + + + + + + 255 + 255 + 255 + + + + + + + 124 + 124 + 124 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 0 + 0 + 0 + + + + + + + 248 + 248 + 248 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + false + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + Qt::Horizontal + + + + 111 + 10 + + + + + + + + + 120 + 28 + + + + Reloads the saved settings into GCS. +Useful if you have accidentally changed some settings. + + + QPushButton { +border: 1px outset #999; +border-radius: 5; +background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255)); +} + +QPushButton:pressed { + + border-style: inset; + background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255)); + +} + + +QPushButton:hover { + border: 1px outset #999; +border-color: rgb(83, 83, 83); +border-radius: 4; + } + + + Reload Board Data + + + + button:reload + buttongroup:10 + + + + + + + + + 60 + 28 + + + + Send settings to the board but do not save to the non-volatile memory + + + QPushButton { +border: 1px outset #999; +border-radius: 5; +background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255)); +} + +QPushButton:pressed { + + border-style: inset; + background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255)); + +} + + +QPushButton:hover { + border: 1px outset #999; +border-color: rgb(83, 83, 83); +border-radius: 4; + } + + + Apply + + + + button:apply + + + + + + + + + 60 + 28 + + + + Send settings to the board and save to the non-volatile memory + + + QPushButton { +border: 1px outset #999; +border-radius: 5; +background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255)); +} + +QPushButton:pressed { + + border-style: inset; + background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255)); + +} + + +QPushButton:hover { + border: 1px outset #999; +border-color: rgb(83, 83, 83); +border-radius: 4; + } + + + Save + + + + button:save + + + + + + + + + + 30 + 440 + 371 + 31 + + + + The buttons below change the autotuning settings which +will alter thingsfor the next autotuning flight + + + + + + 250 + 380 + 171 + 31 + + + + QPushButton { +border: 1px outset #999; +border-radius: 5; +background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255)); +} + +QPushButton:pressed { + + border-style: inset; + background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255)); + +} + + +QPushButton:hover { + border: 1px outset #999; +border-color: rgb(83, 83, 83); +border-radius: 4; + } + + + Apply Computed Values + + + + + + 40 + 410 + 104 + 20 + + + + Step Size + + + + + + 149 + 410 + 266 + 20 + + + + Qt::Horizontal + + + + objname:RelayTuningSettings + fieldname:Amplitude + scale:0.01 + haslimits:no + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/config.pro b/ground/openpilotgcs/src/plugins/config/config.pro index 8e49cb54d..1e0bdb3cb 100644 --- a/ground/openpilotgcs/src/plugins/config/config.pro +++ b/ground/openpilotgcs/src/plugins/config/config.pro @@ -36,7 +36,8 @@ HEADERS += configplugin.h \ cfg_vehicletypes/configfixedwingwidget.h \ cfg_vehicletypes/vehicleconfig.h \ configrevowidget.h \ - config_global.h + config_global.h \ + configautotunewidget.h SOURCES += configplugin.cpp \ configgadgetconfiguration.cpp \ configgadgetwidget.cpp \ @@ -67,7 +68,8 @@ SOURCES += configplugin.cpp \ cfg_vehicletypes/configfixedwingwidget.cpp \ cfg_vehicletypes/configccpmwidget.cpp \ outputchannelform.cpp \ - cfg_vehicletypes/vehicleconfig.cpp + cfg_vehicletypes/vehicleconfig.cpp \ + configautotunewidget.cpp FORMS += airframe.ui \ cc_hw_settings.ui \ pro_hw_settings.ui \ @@ -83,5 +85,13 @@ FORMS += airframe.ui \ outputchannelform.ui \ revosensors.ui \ txpid.ui \ - pipxtreme.ui + pipxtreme.ui \ + autotune.ui RESOURCES += configgadget.qrc + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp new file mode 100644 index 000000000..33106be4c --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp @@ -0,0 +1,136 @@ + +#include "configautotunewidget.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "relaytuningsettings.h" +#include "relaytuning.h" +#include "stabilizationsettings.h" + +ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) : + ConfigTaskWidget(parent) +{ + m_autotune = new Ui_AutotuneWidget(); + m_autotune->setupUi(this); + + // Connect automatic signals + autoLoadWidgets(); + disableMouseWheelEvents(); + + // Whenever any value changes compute new potential stabilization settings + connect(m_autotune->rateTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization())); + connect(m_autotune->attitudeTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization())); + + RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager()); + Q_ASSERT(relayTuning); + if(relayTuning) + connect(relayTuning, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(recomputeStabilization())); + + // Connect the apply button for the stabilization settings + connect(m_autotune->useComputedValues, SIGNAL(pressed()), this, SLOT(saveStabilization())); +} + +/** + * Apply the stabilization settings computed + */ +void ConfigAutotuneWidget::saveStabilization() +{ + StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager()); + Q_ASSERT(stabilizationSettings); + if(!stabilizationSettings) + return; + + // Make sure to recompute in case the other stab settings changed since + // the last time + recomputeStabilization(); + + // Apply this data to the board + stabilizationSettings->setData(stabSettings); + stabilizationSettings->updated(); +} + +/** + * Called whenever the gain ratios or measured values + * are changed + */ +void ConfigAutotuneWidget::recomputeStabilization() +{ + RelayTuningSettings *relayTuningSettings = RelayTuningSettings::GetInstance(getObjectManager()); + Q_ASSERT(relayTuningSettings); + if (!relayTuningSettings) + return; + + RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager()); + Q_ASSERT(relayTuning); + if(!relayTuning) + return; + + StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager()); + Q_ASSERT(stabilizationSettings); + if(!stabilizationSettings) + return; + + RelayTuning::DataFields relayTuningData = relayTuning->getData(); + RelayTuningSettings::DataFields tuningSettingsData = relayTuningSettings->getData(); + stabSettings = stabilizationSettings->getData(); + + // Need to divide these by 100 because that is what the .ui file does + // to get the UAVO + const double gain_ratio_r = m_autotune->rateTuning->value() / 100.0; + const double zero_ratio_r = m_autotune->rateTuning->value() / 100.0; + const double gain_ratio_p = m_autotune->attitudeTuning->value() / 100.0; + const double zero_ratio_p = m_autotune->attitudeTuning->value() / 100.0; + + // For now just run over roll and pitch + for (int i = 0; i < 2; i++) { + if (relayTuningData.Period[i] == 0 || relayTuningData.Gain[i] == 0) + continue; + + double wu = 1000.0 * 2 * M_PI / relayTuningData.Period[i]; // ultimate freq = output osc freq (rad/s) + + double wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) + double zc = wc * zero_ratio_r; // controller zero location (rad/s) + double kpu = 4.0f / M_PI / relayTuningData.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity + double kp = kpu * gain_ratio_r; // proportional gain + double ki = zc * kp; // integral gain + + // Now calculate gains for the next loop out knowing it is the integral of + // the inner loop -- the plant is position/velocity = scale*1/s + double wc2 = wc * gain_ratio_p; // crossover of the attitude loop + double kp2 = wc2; // kp of attitude + double ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude + + switch(i) { + case 0: // Roll + + stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP] = kp; + stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI] = ki; + stabSettings.RollPI[StabilizationSettings::ROLLPI_KP] = kp2; + stabSettings.RollPI[StabilizationSettings::ROLLPI_KI] = ki2; + break; + case 1: // Pitch + stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP] = kp; + stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI] = ki; + stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP] = kp2; + stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI] = ki2; + break; + } + } + + // Display these computed settings + m_autotune->rollRateKp->setText(QString().number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP])); + m_autotune->rollRateKi->setText(QString().number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI])); + m_autotune->rollAttitudeKp->setText(QString().number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KP])); + m_autotune->rollAttitudeKi->setText(QString().number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KI])); + m_autotune->pitchRateKp->setText(QString().number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP])); + m_autotune->pitchRateKi->setText(QString().number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI])); + m_autotune->pitchAttitudeKp->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP])); + m_autotune->pitchAttitudeKi->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI])); +} diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h new file mode 100644 index 000000000..7a89ed373 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h @@ -0,0 +1,60 @@ +/** + ****************************************************************************** + * + * @file configautotunewidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief The Configuration Gadget used to adjust or recalculate autotuning + *****************************************************************************/ +/* + * 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 CONFIGAUTOTUNE_H +#define CONFIGAUTOTUNE_H + +#include "ui_autotune.h" +#include "../uavobjectwidgetutils/configtaskwidget.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "uavobject.h" +#include "stabilizationsettings.h" +#include "relaytuningsettings.h" +#include "relaytuning.h" +#include +#include + +class ConfigAutotuneWidget : public ConfigTaskWidget +{ + Q_OBJECT +public: + explicit ConfigAutotuneWidget(QWidget *parent = 0); + +private: + Ui_AutotuneWidget *m_autotune; + StabilizationSettings::DataFields stabSettings; + +signals: + +public slots: + +private slots: + void recomputeStabilization(); + void saveStabilization(); +}; + +#endif // CONFIGAUTOTUNE_H diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index fe1f517bf..2131f58ea 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -43,7 +43,6 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : ui->setupUi(this); connect(ui->zeroBias,SIGNAL(clicked()),this,SLOT(startAccelCalibration())); - addApplySaveButtons(ui->applyButton,ui->saveButton); addUAVObject("AttitudeSettings"); @@ -64,26 +63,39 @@ ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() } void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { - QMutexLocker locker(&startStop); - ui->zeroBiasProgress->setValue((float) qMin(accelUpdates,gyroUpdates) / NUM_SENSOR_UPDATES * 100); + if (!timer.isActive()) { + // ignore updates that come in after the timer has expired + return; + } Accels * accels = Accels::GetInstance(getObjectManager()); Gyros * gyros = Gyros::GetInstance(getObjectManager()); - if(obj->getObjID() == Accels::OBJID && accelUpdates < NUM_SENSOR_UPDATES) { + // Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES samples + // for both gyros and accels. + // Note that, at present, we stash the samples and then compute the bias + // at the end, even though the mean could be accumulated as we go. + // In future, a better algorithm could be used. + if(obj->getObjID() == Accels::OBJID) { accelUpdates++; Accels::DataFields accelsData = accels->getData(); x_accum.append(accelsData.x); y_accum.append(accelsData.y); z_accum.append(accelsData.z); - } else if (obj->getObjID() == Gyros::OBJID && gyroUpdates < NUM_SENSOR_UPDATES) { + } else if (obj->getObjID() == Gyros::OBJID) { gyroUpdates++; Gyros::DataFields gyrosData = gyros->getData(); x_gyro_accum.append(gyrosData.x); y_gyro_accum.append(gyrosData.y); z_gyro_accum.append(gyrosData.z); - } else if ( accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) { + } + + // update the progress indicator + ui->zeroBiasProgress->setValue((float) qMin(accelUpdates, gyroUpdates) / NUM_SENSOR_UPDATES * 100); + + // If we have enough samples, then stop sampling and compute the biases + if (accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) { timer.stop(); disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); @@ -108,14 +120,13 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject * obj) { attitudeSettingsData.GyroBias[2] = -z_gyro_bias; attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); - } else { - // Possible to get here if weird threading stuff happens. Just ignore updates. - qDebug("Unexpected accel update received."); + + // reenable controls + enableControls(true); } } void ConfigCCAttitudeWidget::timeout() { - QMutexLocker locker(&startStop); UAVDataObject * obj = Accels::GetInstance(getObjectManager()); disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); @@ -131,10 +142,15 @@ void ConfigCCAttitudeWidget::timeout() { msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); + // reset progress indicator + ui->zeroBiasProgress->setValue(0); + // reenable controls + enableControls(true); } void ConfigCCAttitudeWidget::startAccelCalibration() { - QMutexLocker locker(&startStop); + // disable controls during sampling + enableControls(false); accelUpdates = 0; gyroUpdates = 0; @@ -156,28 +172,28 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { connect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); connect(gyros,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); - // Set up timeout timer - timer.start(10000); - connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); - // Speed up updates initialAccelsMdata = accels->getMetadata(); UAVObject::Metadata accelsMdata = initialAccelsMdata; UAVObject::SetFlightTelemetryUpdateMode(accelsMdata, UAVObject::UPDATEMODE_PERIODIC); - accelsMdata.flightTelemetryUpdatePeriod = 30; + accelsMdata.flightTelemetryUpdatePeriod = 30; // ms accels->setMetadata(accelsMdata); initialGyrosMdata = gyros->getMetadata(); UAVObject::Metadata gyrosMdata = initialGyrosMdata; UAVObject::SetFlightTelemetryUpdateMode(gyrosMdata, UAVObject::UPDATEMODE_PERIODIC); - gyrosMdata.flightTelemetryUpdatePeriod = 30; + gyrosMdata.flightTelemetryUpdatePeriod = 30; // ms gyros->setMetadata(gyrosMdata); + // Set up timeout timer + timer.setSingleShot(true); + timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelsMdata.flightTelemetryUpdatePeriod, + gyrosMdata.flightTelemetryUpdatePeriod))); + connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); } void ConfigCCAttitudeWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/CopterControl+Attitude+Configuration", QUrl::StrictMode) ); } @@ -186,7 +202,6 @@ void ConfigCCAttitudeWidget::enableControls(bool enable) if(ui->zeroBias) ui->zeroBias->setEnabled(enable); ConfigTaskWidget::enableControls(enable); - } void ConfigCCAttitudeWidget::updateObjectsFromWidgets() diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index fc1a7f623..0637d268a 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -34,7 +34,6 @@ #include "uavobject.h" #include #include -#include class Ui_Widget; @@ -55,7 +54,6 @@ private slots: void openHelp(); private: - QMutex startStop; Ui_ccattitude *ui; QTimer timer; UAVObject::Metadata initialAccelsMdata; @@ -67,7 +65,7 @@ private: QList x_accum, y_accum, z_accum; QList x_gyro_accum, y_gyro_accum, z_gyro_accum; - static const int NUM_SENSOR_UPDATES = 60; + static const int NUM_SENSOR_UPDATES = 300; static const float ACCEL_SCALE = 0.004f * 9.81f; protected: virtual void enableControls(bool enable); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index 109f1e2a3..5a94ae5b1 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -32,6 +32,7 @@ #include "configinputwidget.h" #include "configoutputwidget.h" #include "configstabilizationwidget.h" +#include "configautotunewidget.h" #include "configcamerastabilizationwidget.h" #include "configtxpidwidget.h" #include "config_pro_hw_widget.h" @@ -84,6 +85,9 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) qwd = new ConfigStabilizationWidget(this); ftw->insertTab(ConfigGadgetWidget::stabilization, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Stabilization")); + qwd = new ConfigAutotuneWidget(this); + ftw->insertTab(ConfigGadgetWidget::autotune, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Autotune")); + qwd = new ConfigCameraStabilizationWidget(this); ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, QIcon(":/configgadget/images/camera.png"), QString("Camera Stab")); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index 7771ce2cd..4f737ff36 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -48,7 +48,7 @@ class ConfigGadgetWidget: public QWidget public: ConfigGadgetWidget(QWidget *parent = 0); ~ConfigGadgetWidget(); - enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, pipxtreme}; + enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, pipxtreme, autotune}; void startInputWizard(); public slots: diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp index 5fa2ead50..4fd33031c 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp @@ -44,25 +44,19 @@ namespace Core { ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidget *modeStack) : - QWidget(mainWindow), // Pip + QWidget(mainWindow), m_availableDevList(0), m_connectBtn(0), m_ioDev(NULL), + polling(true), m_mainWindow(mainWindow) { - // Q_UNUSED(mainWindow); - - /* QVBoxLayout *top = new QVBoxLayout; - top->setSpacing(0); - top->setMargin(0);*/ - QHBoxLayout *layout = new QHBoxLayout; layout->setSpacing(5); layout->setContentsMargins(5,5,5,5); layout->addWidget(new QLabel(tr("Connections:"))); m_availableDevList = new QComboBox; - //m_availableDevList->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); m_availableDevList->setMinimumWidth(100); m_availableDevList->setMaximumWidth(150); m_availableDevList->setContextMenuPolicy(Qt::CustomContextMenu); @@ -72,13 +66,8 @@ ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidge m_connectBtn->setEnabled(false); layout->addWidget(m_connectBtn); - /* Utils::StyledBar *bar = new Utils::StyledBar; - bar->setLayout(layout); - - top->addWidget(bar);*/ setLayout(layout); - // modeStack->insertCornerWidget(modeStack->cornerWidgetCount()-1, this); modeStack->setCornerWidget(this, Qt::TopRightCorner); QObject::connect(m_connectBtn, SIGNAL(pressed()), this, SLOT(onConnectPressed())); @@ -86,8 +75,8 @@ ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidge ConnectionManager::~ConnectionManager() { - disconnectDevice(); // Pip - suspendPolling(); // Pip + disconnectDevice(); + suspendPolling(); } void ConnectionManager::init() @@ -101,9 +90,13 @@ void ConnectionManager::init() /** * Method called when the user clicks the "Connect" button */ -bool ConnectionManager::connectDevice(devListItem device) +bool ConnectionManager::connectDevice(DevListItem device) { - QIODevice *io_dev = device.connection->openDevice(device.Name); + DevListItem connection_device = findDevice(m_availableDevList->itemData(m_availableDevList->currentIndex(),Qt::ToolTipRole).toString()); + if (!connection_device.connection) + return false; + + QIODevice *io_dev = connection_device.connection->openDevice(connection_device.device.name); if (!io_dev) return false; @@ -111,17 +104,6 @@ bool ConnectionManager::connectDevice(devListItem device) // check if opening the device worked if (!io_dev->isOpen()) { - qDebug() << "Error: io_dev->isOpen() returned FALSE .. could not open connection to " << device.devName - << ": " << io_dev->errorString(); - - // close the device - // EDOUARD: why do we close if we could not open ??? - try { - device.connection->closeDevice(device.devName); - } - catch (...) { // handle exception - qDebug() << "Exception: connection_device.connection->closeDevice(" << device.devName << ")"; - } return false; } @@ -133,7 +115,7 @@ bool ConnectionManager::connectDevice(devListItem device) connect(m_connectionDevice.connection, SIGNAL(destroyed(QObject *)), this, SLOT(onConnectionDestroyed(QObject *)), Qt::QueuedConnection); // signal interested plugins that we connected to the device - emit deviceConnected(m_ioDev); + emit deviceConnected(io_dev); m_connectBtn->setText("Disconnect"); m_availableDevList->setEnabled(false); return true; @@ -158,10 +140,11 @@ bool ConnectionManager::disconnectDevice() emit deviceAboutToDisconnect(); try { - if (m_connectionDevice.connection) - m_connectionDevice.connection->closeDevice(m_connectionDevice.devName); + if (m_connectionDevice.connection) { + m_connectionDevice.connection->closeDevice(m_connectionDevice.getConName()); + } } catch (...) { // handle exception - qDebug() << "Exception: m_connectionDevice.connection->closeDevice(" << m_connectionDevice.devName << ")"; + qDebug() << "Exception: m_connectionDevice.connection->closeDevice(" << m_connectionDevice.getConName() << ")"; } m_connectionDevice.connection = NULL; @@ -184,9 +167,6 @@ void ConnectionManager::objectAdded(QObject *obj) IConnection *connection = Aggregation::query(obj); if (!connection) return; - //qDebug() << "Connection object registered:" << connection->connectionName(); - //qDebug() << connection->availableDevices(); - //register devices and populate CB devChanged(connection); @@ -203,8 +183,9 @@ void ConnectionManager::aboutToRemoveObject(QObject *obj) IConnection *connection = Aggregation::query(obj); if (!connection) return; - if (m_connectionDevice.connection && m_connectionDevice.connection == connection) // Pip + if (m_connectionDevice.connection && m_connectionDevice.connection == connection) { // we are currently using the one that is about to be removed + disconnectDevice(); m_connectionDevice.connection = NULL; m_ioDev = NULL; } @@ -214,10 +195,9 @@ void ConnectionManager::aboutToRemoveObject(QObject *obj) } -void ConnectionManager::onConnectionDestroyed(QObject *obj) // Pip +void ConnectionManager::onConnectionDestroyed(QObject *obj) { Q_UNUSED(obj) - //onConnectionClosed(obj); disconnectDevice(); } @@ -230,7 +210,7 @@ void ConnectionManager::onConnectPressed() if (!m_ioDev) { // connecting to currently selected device - devListItem device = findDevice(m_availableDevList->itemData(m_availableDevList->currentIndex(), Qt::ToolTipRole).toString()); + DevListItem device = findDevice(m_availableDevList->itemData(m_availableDevList->currentIndex(), Qt::ToolTipRole).toString()); if (device.connection) connectDevice(device); } @@ -243,17 +223,17 @@ void ConnectionManager::onConnectPressed() /** * Find a device by its displayed (visible on screen) name */ -devListItem ConnectionManager::findDevice(const QString &devName) +DevListItem ConnectionManager::findDevice(const QString &devName) { - foreach (devListItem d, m_devList) + foreach (DevListItem d, m_devList) { - if (d.devName == devName) + if (d.getConName() == devName) return d; } qDebug() << "findDevice: cannot find " << devName << " in device list"; - devListItem d; + DevListItem d; d.connection = NULL; return d; } @@ -272,6 +252,7 @@ void ConnectionManager::suspendPolling() m_connectBtn->setEnabled(false); m_availableDevList->setEnabled(false); + polling = false; } /** @@ -287,29 +268,50 @@ void ConnectionManager::resumePolling() m_connectBtn->setEnabled(true); m_availableDevList->setEnabled(true); + polling = true; } /** -* Unregister all devices from one connection plugin -* \param[in] connection Connection type that you want to forget about :) -*/ -void ConnectionManager::unregisterAll(IConnection *connection) + * Synchronize the list of connections displayed with those physically + * present + * @param[in] connection Connection type that you want to forget about :) + */ +void ConnectionManager::updateConnectionList(IConnection *connection) { - for (QLinkedList::iterator iter = m_devList.begin(); iter != m_devList.end(); ) + // Get the updated list of devices + QList availableDev = connection->availableDevices(); + + // Go through the list of connections of that type. If they are not in the + // available device list then remove them. If they are connected, then + // disconnect them. + for (QLinkedList::iterator iter = m_devList.begin(); iter != m_devList.end(); ) { - if (iter->connection == connection) - { - if (m_connectionDevice.connection && m_connectionDevice.connection == connection) - { // we are currently using the one we are about to erase - //onConnectionClosed(m_connectionDevice.connection); + if (iter->connection != connection) { + ++iter; + continue; + } + + // See if device exists in the updated availability list + bool found = availableDev.contains(iter->device); + if (!found) { + // we are currently using the one we are about to erase + if (m_connectionDevice.connection && m_connectionDevice.connection == connection && m_connectionDevice.device == iter->device) { disconnectDevice(); } iter = m_devList.erase(iter); - } - else + } else ++iter; } + + // Add any back to list that don't exist + foreach (IConnection::device dev, availableDev) + { + bool found = m_devList.contains(DevListItem(connection, dev)); + if (!found) { + registerDevice(connection,dev); + } + } } /** @@ -318,13 +320,9 @@ void ConnectionManager::unregisterAll(IConnection *connection) * @param disp is the name that is displayed in the dropdown menu * @param name is the actual device name */ -void ConnectionManager::registerDevice(IConnection *conn, const QString &devN, const QString &name, const QString &disp) +void ConnectionManager::registerDevice(IConnection *conn, IConnection::device device) { - devListItem d; - d.connection = conn; - d.devName = devN; - d.Name = name; - d.displayName=disp; + DevListItem d(conn,device); m_devList.append(d); } @@ -345,45 +343,13 @@ void ConnectionManager::devChanged(IConnection *connection) m_availableDevList->clear(); //remove registered devices of this IConnection from the list - unregisterAll(connection); + updateConnectionList(connection); - //and add them back in the list - QList availableDev = connection->availableDevices(); - foreach (IConnection::device dev, availableDev) - { - QString cbName = connection->shortName() + ": " + dev.name; - QString disp = connection->shortName() + " : " + dev.displayName; - registerDevice(connection,cbName,dev.name,disp); - } + updateConnectionDropdown(); qDebug() << "# devices " << m_devList.count(); emit availableDevicesChanged(m_devList); - //add all the list again to the combobox - foreach (devListItem device, m_devList) - { - m_availableDevList->addItem(device.displayName); - m_availableDevList->setItemData(m_availableDevList->count() - 1, (const QString)device.devName,Qt::ToolTipRole); - if(!m_ioDev && device.displayName.startsWith("USB")) - { - if(m_mainWindow->generalSettings()->autoConnect() || m_mainWindow->generalSettings()->autoSelect()) - m_availableDevList->setCurrentIndex(m_availableDevList->count() - 1); - - if(m_mainWindow->generalSettings()->autoConnect()) - { - connectDevice(device); - qDebug() << "ConnectionManager::devChanged autoconnected USB device"; - } - } - } - if(m_ioDev)//if a device is connected make it the one selected on the dropbox - { - for(int x=0; x < m_availableDevList->count(); ++x) - { - if(m_connectionDevice.devName==m_availableDevList->itemData(x,Qt::ToolTipRole).toString()) - m_availableDevList->setCurrentIndex(x); - } - } //disable connection button if the liNameif (m_availableDevList->count() > 0) if (m_availableDevList->count() > 0) @@ -392,6 +358,35 @@ void ConnectionManager::devChanged(IConnection *connection) m_connectBtn->setEnabled(false); } +void ConnectionManager::updateConnectionDropdown() +{ + //add all the list again to the combobox + foreach (DevListItem d, m_devList) + { + m_availableDevList->addItem(d.getConName()); + m_availableDevList->setItemData(m_availableDevList->count()-1, d.getConName(), Qt::ToolTipRole); + if(!m_ioDev && d.getConName().startsWith("USB")) + { + if(m_mainWindow->generalSettings()->autoConnect() || m_mainWindow->generalSettings()->autoSelect()) + m_availableDevList->setCurrentIndex(m_availableDevList->count() - 1); + + if(m_mainWindow->generalSettings()->autoConnect() && polling) + { + qDebug() << "Automatically opening device"; + connectDevice(d); + qDebug()<<"ConnectionManager::devChanged autoconnected USB device"; + } + } + } + if(m_ioDev)//if a device is connected make it the one selected on the dropbox + { + for(int x=0; x < m_availableDevList->count(); ++x) + { + if(m_connectionDevice.getConName()==m_availableDevList->itemData(x,Qt::ToolTipRole).toString()) + m_availableDevList->setCurrentIndex(x); + } + } +} void Core::ConnectionManager::connectionsCallBack() @@ -403,4 +398,5 @@ void Core::ConnectionManager::connectionsCallBack() connectionBackup.clear(); disconnect(ExtensionSystem::PluginManager::instance(),SIGNAL(pluginsLoadEnded()),this,SLOT(connectionsCallBack())); } + } //namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h index 4d57319ae..e09a0c863 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h @@ -32,6 +32,7 @@ #include #include "mainwindow.h" #include "generalsettings.h" +#include #include #include #include @@ -55,12 +56,26 @@ class MainWindow; } // namespace Internal -struct devListItem +class DevListItem { +public: + DevListItem(IConnection *c, IConnection::device d) : + connection(c), device(d) { } + + DevListItem() : connection(NULL) { } + + QString getConName() { + if (connection == NULL) + return ""; + return connection->shortName() + ": " + device.displayName; + } + + bool operator==(const DevListItem &rhs) { + return connection == rhs.connection && device == rhs.device; + } + IConnection *connection; - QString devName; - QString Name; - QString displayName; + IConnection::device device; }; @@ -75,28 +90,29 @@ public: void init(); QIODevice* getCurrentConnection() { return m_ioDev; } - devListItem getCurrentDevice() { return m_connectionDevice; } - devListItem findDevice(const QString &devName); + DevListItem getCurrentDevice() { return m_connectionDevice; } + DevListItem findDevice(const QString &devName); - QLinkedList getAvailableDevices() { return m_devList; } + QLinkedList getAvailableDevices() { return m_devList; } bool isConnected() { return m_ioDev != 0; } - bool connectDevice(devListItem device); + bool connectDevice(DevListItem device); bool disconnectDevice(); void suspendPolling(); void resumePolling(); protected: - void unregisterAll(IConnection *connection); - void registerDevice(IConnection *conn, const QString &devN, const QString &name, const QString &disp); + void updateConnectionList(IConnection *connection); + void registerDevice(IConnection *conn, IConnection::device device); + void updateConnectionDropdown(); signals: - void deviceConnected(QIODevice *dev); + void deviceConnected(QIODevice *device); void deviceDisconnected(); void deviceAboutToDisconnect(); - void availableDevicesChanged(const QLinkedList devices); + void availableDevicesChanged(const QLinkedList devices); private slots: void objectAdded(QObject *obj); @@ -105,22 +121,23 @@ private slots: void onConnectPressed(); void devChanged(IConnection *connection); - // void onConnectionClosed(QObject *obj); - void onConnectionDestroyed(QObject *obj); + void onConnectionDestroyed(QObject *obj); void connectionsCallBack(); //used to call devChange after all the plugins are loaded protected: QComboBox *m_availableDevList; QPushButton *m_connectBtn; - QLinkedList m_devList; + QLinkedList m_devList; QList m_connectionsList; //currently connected connection plugin - devListItem m_connectionDevice; + DevListItem m_connectionDevice; //currently connected QIODevice QIODevice *m_ioDev; private: + bool connectDevice(); + bool polling; Internal::MainWindow *m_mainWindow; QList connectionBackup; diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp index 04339b713..c4d95ec4b 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp @@ -174,10 +174,8 @@ QList IPconnectionConnection::availableDevices() return list; } -QIODevice *IPconnectionConnection::openDevice(const QString &deviceName) +QIODevice *IPconnectionConnection::openDevice(const QString &) { - Q_UNUSED(deviceName); - QString HostName; int Port; bool UseTCP; @@ -210,10 +208,8 @@ QIODevice *IPconnectionConnection::openDevice(const QString &deviceName) return ipSocket; } -void IPconnectionConnection::closeDevice(const QString &deviceName) +void IPconnectionConnection::closeDevice(const QString &) { - Q_UNUSED(deviceName); - if (ipSocket){ ipConMutex.lock(); emit CloseSocket(ipSocket); diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp.orig b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp.orig deleted file mode 100644 index 2ff775e31..000000000 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp.orig +++ /dev/null @@ -1,512 +0,0 @@ -/** - ****************************************************************************** - * - * @file notifypluginoptionspage.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Notify Plugin options page - * @see The GNU Public License (GPL) Version 3 - * @defgroup notifyplugin - * @{ - * - *****************************************************************************/ -/* - * 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 - */ - -#include "notifypluginoptionspage.h" -#include -#include "notificationitem.h" -#include "ui_notifypluginoptionspage.h" -#include "extensionsystem/pluginmanager.h" -#include "utils/pathutils.h" - -#include -#include -#include -#include -#include -#include -#include - -#include "notifyplugin.h" -#include "notifyitemdelegate.h" -#include "notifytablemodel.h" -#include "notifylogging.h" - -NotifyPluginOptionsPage::NotifyPluginOptionsPage(/*NotificationItem *config,*/ QObject *parent) - : IOptionsPage(parent) - , objManager(*ExtensionSystem::PluginManager::instance()->getObject()) - , owner(qobject_cast(parent)) - , currentCollectionPath("") -{ -} - -NotifyPluginOptionsPage::~NotifyPluginOptionsPage() -{ -} - -//creates options page widget (uses the UI file) -QWidget *NotifyPluginOptionsPage::createPage(QWidget *parent) -{ - options_page.reset(new Ui::NotifyPluginOptionsPage()); - //main widget - QWidget *optionsPageWidget = new QWidget; - //main layout - options_page->setupUi(optionsPageWidget); - - listSoundFiles.clear(); - - options_page->SoundDirectoryPathChooser->setExpectedKind(Utils::PathChooser::Directory); - options_page->SoundDirectoryPathChooser->setPromptDialogTitle(tr("Choose sound collection directory")); - - // Fills the combo boxes for the UAVObjects - QList< QList > objList = objManager.getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { - options_page->UAVObject->addItem(obj->getName()); - } - } - -<<<<<<< Updated upstream - connect(options_page->SoundDirectoryPathChooser, SIGNAL(changed(const QString&)), this, SLOT(on_buttonSoundFolder_clicked(const QString&))); - connect(options_page->SoundCollectionList, SIGNAL(currentIndexChanged (int)), this, SLOT(on_soundLanguage_indexChanged(int))); - connect(options_page->UAVObject, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_UAVObject_indexChanged(QString))); -======= - options_page = new Ui::NotifyPluginOptionsPage(); - //main widget - QWidget *optionsPageWidget = new QWidget; - //main layout - options_page->setupUi(optionsPageWidget); - - delegateItems.clear(); - listSoundFiles.clear(); - - options_page->horizontalLayout_3->addWidget(new QPushButton("testtt")); - delegateItems << "Repeat Once" - << "Repeat Instantly" - << "Repeat 10 seconds" - << "Repeat 30 seconds" - << "Repeat 1 minute"; - - options_page->chkEnableSound->setChecked(owner->getEnableSound()); - options_page->SoundDirectoryPathChooser->setExpectedKind(Utils::PathChooser::Directory); - options_page->SoundDirectoryPathChooser->setPromptDialogTitle(tr("Choose sound collection directory")); - - - - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - objManager = pm->getObject(); - - // Fills the combo boxes for the UAVObjects - QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { - options_page->UAVObject->addItem(obj->getName()); - } - } - - connect(options_page->SoundDirectoryPathChooser, SIGNAL(changed(const QString&)), this, SLOT(on_buttonSoundFolder_clicked(const QString&))); - connect(options_page->SoundCollectionList, SIGNAL(currentIndexChanged (int)), this, SLOT(on_soundLanguage_indexChanged(int))); - connect(options_page->buttonAdd, SIGNAL(pressed()), this, SLOT(on_buttonAddNotification_clicked())); - connect(options_page->buttonDelete, SIGNAL(pressed()), this, SLOT(on_buttonDeleteNotification_clicked())); - connect(options_page->buttonModify, SIGNAL(pressed()), this, SLOT(on_buttonModifyNotification_clicked())); -// connect(options_page->buttonTestSound1, SIGNAL(clicked()), this, SLOT(on_buttonTestSound1_clicked())); -// connect(options_page->buttonTestSound2, SIGNAL(clicked()), this, SLOT(on_buttonTestSound2_clicked())); - connect(options_page->buttonPlayNotification, SIGNAL(clicked()), this, SLOT(on_buttonTestSoundNotification_clicked())); - connect(options_page->chkEnableSound, SIGNAL(toggled(bool)), this, SLOT(on_chkEnableSound_toggled(bool))); - - - connect(options_page->UAVObject, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_UAVObject_indexChanged(QString))); - connect(this, SIGNAL(updateNotifications(QList)), - owner, SLOT(updateNotificationList(QList))); - connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification())); - - //emit resetNotification(); - - - privListNotifications.clear(); - - for (int i = 0; i < owner->getListNotifications().size(); ++i) { - NotifyPluginConfiguration* notification = new NotifyPluginConfiguration(); - owner->getListNotifications().at(i)->copyTo(notification); - privListNotifications.append(notification); - } - - updateConfigView(owner->getCurrentNotification()); - - options_page->chkEnableSound->setChecked(owner->getEnableSound()); - - QStringList headerStrings; - headerStrings << "Name" << "Repeats" << "Lifetime,sec"; ->>>>>>> Stashed changes - - connect(this, SIGNAL(updateNotifications(QList)), - owner, SLOT(updateNotificationList(QList))); - //connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification())); - - -// privListNotifications = ((qobject_cast(parent))->getListNotifications()); - privListNotifications = owner->getListNotifications(); - - updateConfigView(owner->getCurrentNotification()); - - initRulesTable(); - initButtons(); - initPhononPlayer(); - - return optionsPageWidget; -} - -void NotifyPluginOptionsPage::initButtons() -{ - options_page->chkEnableSound->setChecked(owner->getEnableSound()); - connect(options_page->chkEnableSound, SIGNAL(toggled(bool)), this, SLOT(on_chkEnableSound_toggled(bool))); - - options_page->buttonModify->setEnabled(false); - options_page->buttonDelete->setEnabled(false); - options_page->buttonPlayNotification->setEnabled(false); - connect(options_page->buttonAdd, SIGNAL(pressed()), this, SLOT(on_buttonAddNotification_clicked())); - connect(options_page->buttonDelete, SIGNAL(pressed()), this, SLOT(on_buttonDeleteNotification_clicked())); - connect(options_page->buttonModify, SIGNAL(pressed()), this, SLOT(on_buttonModifyNotification_clicked())); - connect(options_page->buttonPlayNotification, SIGNAL(clicked()), this, SLOT(on_buttonTestSoundNotification_clicked())); -} - -void NotifyPluginOptionsPage::initPhononPlayer() -{ - notifySound.reset(Phonon::createPlayer(Phonon::NotificationCategory)); - connect(notifySound.data(),SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this,SLOT(changeButtonText(Phonon::State,Phonon::State))); - connect(notifySound.data(), SIGNAL(finished(void)), this, SLOT(onFinishedPlaying(void))); -} - -void NotifyPluginOptionsPage::initRulesTable() -{ - qNotifyDebug_if(_notifyRulesModel.isNull()) << "_notifyRulesModel.isNull())"; - qNotifyDebug_if(!_notifyRulesSelection) << "_notifyRulesSelection.isNull())"; - //QItemSelectionModel* selection = _notifyRulesSelection.take(); - _notifyRulesModel.reset(new NotifyTableModel(privListNotifications)); - _notifyRulesSelection = new QItemSelectionModel(_notifyRulesModel.data()); - - connect(_notifyRulesSelection, SIGNAL(selectionChanged ( const QItemSelection &, const QItemSelection & )), - this, SLOT(on_tableNotification_changeSelection( const QItemSelection & , const QItemSelection & ))); - connect(this, SIGNAL(entryUpdated(int)), - _notifyRulesModel.data(), SLOT(entryUpdated(int))); - connect(this, SIGNAL(entryAdded(int)), - _notifyRulesModel.data(), SLOT(entryAdded(int))); - - options_page->notifyRulesView->setModel(_notifyRulesModel.data()); - options_page->notifyRulesView->setSelectionModel(_notifyRulesSelection); - options_page->notifyRulesView->setItemDelegate(new NotifyItemDelegate(this)); - - options_page->notifyRulesView->resizeRowsToContents(); - options_page->notifyRulesView->setColumnWidth(eMESSAGE_NAME,200); - options_page->notifyRulesView->setColumnWidth(eREPEAT_VALUE,120); - options_page->notifyRulesView->setColumnWidth(eEXPIRE_TIME,100); - options_page->notifyRulesView->setColumnWidth(eENABLE_NOTIFICATION,60); - options_page->notifyRulesView->setDragEnabled(true); - options_page->notifyRulesView->setAcceptDrops(true); - options_page->notifyRulesView->setDropIndicatorShown(true); - options_page->notifyRulesView->setDragDropMode(QAbstractItemView::InternalMove); - - -} - -void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem* notification) -{ - notification->setSoundCollectionPath(options_page->SoundDirectoryPathChooser->path()); - notification->setCurrentLanguage(options_page->SoundCollectionList->currentText()); - notification->setDataObject(options_page->UAVObject->currentText()); - notification->setObjectField(options_page->UAVObjectField->currentText()); - notification->setSound1(options_page->Sound1->currentText()); - notification->setSound2(options_page->Sound2->currentText()); - notification->setSound3(options_page->Sound3->currentText()); - notification->setSayOrder(options_page->SayOrder->currentText()); - notification->setValue(options_page->Value->currentText()); - notification->setSpinBoxValue(options_page->ValueSpinBox->value()); -} - -/*! -* Called when the user presses apply or OK. -* Saves the current values -*/ -void NotifyPluginOptionsPage::apply() -{ - getOptionsPageValues(owner->getCurrentNotification()); - owner->setEnableSound(options_page->chkEnableSound->isChecked()); - emit updateNotifications(privListNotifications); -} - -void NotifyPluginOptionsPage::finish() -{ - disconnect(notifySound.data(),SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this,SLOT(changeButtonText(Phonon::State,Phonon::State))); - if (notifySound) { - notifySound->stop(); - notifySound->clear(); - } -} - -////////////////////////////////////////////////////////////////////////////// -// Fills in the combo box when value is changed in the -// combo box -////////////////////////////////////////////////////////////////////////////// -void NotifyPluginOptionsPage::on_UAVObject_indexChanged(QString val) { - options_page->UAVObjectField->clear(); - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager* objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - options_page->UAVObjectField->addItem(field->getName()); - } -} - -// locate collection folder on disk -void NotifyPluginOptionsPage::on_buttonSoundFolder_clicked(const QString& path) -{ - QDir dirPath(path); - listDirCollections = dirPath.entryList(QDir::AllDirs | QDir::NoDotAndDotDot); - options_page->SoundCollectionList->clear(); - options_page->SoundCollectionList->addItems(listDirCollections); -} - - -void NotifyPluginOptionsPage::on_soundLanguage_indexChanged(int index) -{ - options_page->SoundCollectionList->setCurrentIndex(index); - - currentCollectionPath = options_page->SoundDirectoryPathChooser->path() - + QDir::toNativeSeparators("/" + options_page->SoundCollectionList->currentText()); - - QDir dirPath(currentCollectionPath); - QStringList filters; - filters << "*.mp3" << "*.wav"; - dirPath.setNameFilters(filters); - listSoundFiles = dirPath.entryList(filters); - listSoundFiles.replaceInStrings(QRegExp(".mp3|.wav"), ""); - options_page->Sound1->clear(); - options_page->Sound2->clear(); - options_page->Sound3->clear(); - options_page->Sound1->addItems(listSoundFiles); - options_page->Sound2->addItem(""); - options_page->Sound2->addItems(listSoundFiles); - options_page->Sound3->addItem(""); - options_page->Sound3->addItems(listSoundFiles); - -} - -void NotifyPluginOptionsPage::changeButtonText(Phonon::State newstate, Phonon::State oldstate) -{ - //Q_ASSERT(Phonon::ErrorState != newstate); - - if (newstate == Phonon::PausedState || newstate == Phonon::StoppedState) { - options_page->buttonPlayNotification->setText("Play"); - options_page->buttonPlayNotification->setIcon(QPixmap(":/notify/images/play.png")); - } else { - if (newstate == Phonon::PlayingState) { - options_page->buttonPlayNotification->setText("Stop"); - options_page->buttonPlayNotification->setIcon(QPixmap(":/notify/images/stop.png")); - } - } -} - -void NotifyPluginOptionsPage::onFinishedPlaying() -{ - notifySound->clear(); -} - -void NotifyPluginOptionsPage::on_buttonTestSoundNotification_clicked() -{ - NotificationItem* notification = NULL; - - if (-1 == _notifyRulesSelection->currentIndex().row()) - return; - notifySound->clearQueue(); - notification = privListNotifications.at(_notifyRulesSelection->currentIndex().row()); - notification->parseNotifyMessage(); - QStringList sequence = notification->getMessageSequence(); - Q_ASSERT(!!sequence.size()); - foreach(QString item, sequence) - notifySound->enqueue(Phonon::MediaSource(item)); - - notifySound->play(); -} - -void NotifyPluginOptionsPage::on_chkEnableSound_toggled(bool state) -{ - bool state1 = 1^state; - - QList listOutputs = notifySound->outputPaths(); - Phonon::AudioOutput * audioOutput = (Phonon::AudioOutput*)listOutputs.last().sink(); - audioOutput->setMuted(state1); -} - -void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) -{ - QString path = notification->getSoundCollectionPath(); - if (path == "") { - //QDir dir = QDir::currentPath(); - //path = QDir::currentPath().left(QDir::currentPath().indexOf("OpenPilot",0,Qt::CaseSensitive))+"../share/sounds"; - path = Utils::PathUtils().InsertDataPath("%%DATAPATH%%sounds"); - } - - options_page->SoundDirectoryPathChooser->setPath(path); - - if (-1 != options_page->SoundCollectionList->findText(notification->getCurrentLanguage())){ - options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText(notification->getCurrentLanguage())); - } else { - options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); - } - - if (options_page->UAVObject->findText(notification->getDataObject())!=-1){ - options_page->UAVObject->setCurrentIndex(options_page->UAVObject->findText(notification->getDataObject())); - } - - // Now load the object field values: - options_page->UAVObjectField->clear(); - QString uavDataObject = notification->getDataObject(); - UAVDataObject* obj = dynamic_cast(objManager.getObject(uavDataObject)); - if (obj != NULL ) { - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - options_page->UAVObjectField->addItem(field->getName()); - } - } - - if (-1 != options_page->UAVObjectField->findText(notification->getObjectField())) { - options_page->UAVObjectField->setCurrentIndex(options_page->UAVObjectField->findText(notification->getObjectField())); - } - - if (-1 != options_page->Sound1->findText(notification->getSound1())) { - options_page->Sound1->setCurrentIndex(options_page->Sound1->findText(notification->getSound1())); - } else { - // show item from default location - options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); - options_page->Sound1->setCurrentIndex(options_page->Sound1->findText(notification->getSound1())); - - // don't show item if it wasn't find in stored location - //options_page->Sound1->setCurrentIndex(-1); - } - - if (-1 != options_page->Sound2->findText(notification->getSound2())) { - options_page->Sound2->setCurrentIndex(options_page->Sound2->findText(notification->getSound2())); - } else { - // show item from default location - options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); - options_page->Sound2->setCurrentIndex(options_page->Sound2->findText(notification->getSound2())); - - // don't show item if it wasn't find in stored location - //options_page->Sound2->setCurrentIndex(-1); - } - - if (-1 != options_page->Sound3->findText(notification->getSound3())) { - options_page->Sound3->setCurrentIndex(options_page->Sound3->findText(notification->getSound3())); - } else { - // show item from default location - options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); - options_page->Sound3->setCurrentIndex(options_page->Sound3->findText(notification->getSound3())); - } - - if (-1 != options_page->Value->findText(notification->getValue())) { - options_page->Value->setCurrentIndex(options_page->Value->findText(notification->getValue())); - } - - if (-1 != options_page->SayOrder->findText(notification->getSayOrder())) { - options_page->SayOrder->setCurrentIndex(options_page->SayOrder->findText(notification->getSayOrder())); - } - - options_page->ValueSpinBox->setValue(notification->getSpinBoxValue()); -} - -void NotifyPluginOptionsPage::on_tableNotification_changeSelection( const QItemSelection & selected, const QItemSelection & deselected ) -{ - bool select = false; - notifySound->stop(); - if (selected.indexes().size()) { - select = true; - updateConfigView(privListNotifications.at(selected.indexes().at(0).row())); - } - - options_page->buttonModify->setEnabled(select); - options_page->buttonDelete->setEnabled(select); - options_page->buttonPlayNotification->setEnabled(select); -} - - -void NotifyPluginOptionsPage::on_buttonAddNotification_clicked() -{ - NotificationItem* notification = new NotificationItem; - - if (options_page->SoundDirectoryPathChooser->path()=="") { - QPalette textPalette=options_page->SoundDirectoryPathChooser->palette(); - textPalette.setColor(QPalette::Normal,QPalette::Text, Qt::red); - options_page->SoundDirectoryPathChooser->setPalette(textPalette); - options_page->SoundDirectoryPathChooser->setPath("please select sound collection folder"); - return; - } - - notification->setSoundCollectionPath(options_page->SoundDirectoryPathChooser->path()); - notification->setCurrentLanguage(options_page->SoundCollectionList->currentText()); - notification->setDataObject(options_page->UAVObject->currentText()); - notification->setObjectField(options_page->UAVObjectField->currentText()); - notification->setValue(options_page->Value->currentText()); - notification->setSpinBoxValue(options_page->ValueSpinBox->value()); - - if (options_page->Sound1->currentText().size() > 0) - notification->setSound1(options_page->Sound1->currentText()); - - notification->setSound2(options_page->Sound2->currentText()); - notification->setSound3(options_page->Sound3->currentText()); - -if ( ((!options_page->Sound2->currentText().size()) && (options_page->SayOrder->currentText()=="After second")) - || ((!options_page->Sound3->currentText().size()) && (options_page->SayOrder->currentText()=="After third")) ) { - return; - } else { - notification->setSayOrder(options_page->SayOrder->currentText()); - } - privListNotifications.append(notification); - emit entryAdded(privListNotifications.size() - 1); - _notifyRulesSelection->setCurrentIndex(_notifyRulesModel->index(privListNotifications.size()-1,0,QModelIndex()), - QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); -} - - -void NotifyPluginOptionsPage::on_buttonDeleteNotification_clicked() -{ - _notifyRulesModel->removeRow(_notifyRulesSelection->currentIndex().row()); - if (!_notifyRulesModel->rowCount() - && (_notifyRulesSelection->currentIndex().row() > 0 - && _notifyRulesSelection->currentIndex().row() < _notifyRulesModel->rowCount()) ) - { - options_page->buttonDelete->setEnabled(false); - options_page->buttonModify->setEnabled(false); - options_page->buttonPlayNotification->setEnabled(false); - } - -} - -void NotifyPluginOptionsPage::on_buttonModifyNotification_clicked() -{ - NotificationItem* notification = new NotificationItem; - getOptionsPageValues(notification); - notification->setRetryString(privListNotifications.at(_notifyRulesSelection->currentIndex().row())->retryString()); - notification->setLifetime(privListNotifications.at(_notifyRulesSelection->currentIndex().row())->lifetime()); - notification->setMute(privListNotifications.at(_notifyRulesSelection->currentIndex().row())->mute()); - - privListNotifications.replace(_notifyRulesSelection->currentIndex().row(),notification); - entryUpdated(_notifyRulesSelection->currentIndex().row()); - -} - diff --git a/ground/openpilotgcs/src/plugins/plugins.pro b/ground/openpilotgcs/src/plugins/plugins.pro index c2c7c922b..a0af1cb7c 100644 --- a/ground/openpilotgcs/src/plugins/plugins.pro +++ b/ground/openpilotgcs/src/plugins/plugins.pro @@ -106,6 +106,7 @@ SUBDIRS += plugin_lineardial plugin_systemhealth.subdir = systemhealth plugin_systemhealth.depends = plugin_coreplugin plugin_systemhealth.depends += plugin_uavobjects +plugin_systemhealth.depends += plugin_uavtalk SUBDIRS += plugin_systemhealth #Config gadget @@ -214,6 +215,7 @@ SUBDIRS += plugin_uavobjectwidgetutils plugin_setupwizard.subdir = setupwizard plugin_setupwizard.depends = plugin_coreplugin plugin_setupwizard.depends += plugin_uavobjects +plugin_setupwizard.depends += plugin_config plugin_setupwizard.depends += plugin_uavsettingsimportexport SUBDIRS += plugin_setupwizard diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h index 3ff096db5..f981f6a94 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h @@ -32,11 +32,15 @@ #include #include #include +#include #include #include "rawhid_global.h" #if defined( Q_OS_MAC) +#include +#include +#include #elif defined(Q_OS_UNIX) //#elif defined(Q_OS_LINUX) @@ -96,22 +100,43 @@ public: pjrc_rawhid(); ~pjrc_rawhid(); int open(int max, int vid, int pid, int usage_page, int usage); - int receive(int num, void *buf, int len, int timeout); + int receive(int, void *buf, int len, int timeout); void close(int num); int send(int num, void *buf, int len, int timeout); QString getserial(int num); - void mytest(int num); signals: - void deviceUnplugged(int);//just to make pips changes compile -#if defined( Q_OS_MAC) - -#endif + void deviceUnplugged(int); private: #if defined( Q_OS_MAC) + // Static callbacks called by the HID system with handles to the PJRC object + static void attach_callback(void *, IOReturn, void *, IOHIDDeviceRef); + static void dettach_callback(void *, IOReturn, void *hid_mgr, IOHIDDeviceRef dev); + static void input_callback(void *, IOReturn, void *, IOHIDReportType, uint32_t, uint8_t *, CFIndex); + static void timeout_callback(CFRunLoopTimerRef, void *); + + // Non static methods to call into + void attach(IOHIDDeviceRef dev); + void dettach(IOHIDDeviceRef dev); + void input(uint8_t *, CFIndex); + + // Platform specific handles for the USB device + IOHIDManagerRef hid_manager; + IOHIDDeviceRef dev; + CFRunLoopRef the_correct_runloop; + CFRunLoopRef received_runloop; + + static const int BUFFER_SIZE = 64; + uint8_t buffer[BUFFER_SIZE]; + int attach_count; + int buffer_count; + bool device_open; + bool unplugged; + + QMutex *m_writeMutex; + QMutex *m_readMutex; #elif defined(Q_OS_UNIX) - //#elif defined(Q_OS_LINUX) hid_t *first_hid; hid_t *last_hid; diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp index 490ea2fdd..f5fc3eb4a 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp @@ -40,102 +40,67 @@ #include "pjrc_rawhid.h" #include -#include -#include -#include #include #include #include #include -class delay : public QThread +struct timeout_info { + CFRunLoopRef loopRef; + bool timed_out; +}; + +pjrc_rawhid::pjrc_rawhid() : + device_open(false), hid_manager(NULL), buffer_count(0), unplugged(false) { -public: - static void msleep(unsigned long msecs) - { - QThread::msleep(msecs); - } -}; - -#define BUFFER_SIZE 64 - -//#define printf qDebug -#define printf - -typedef struct hid_struct hid_t; -typedef struct buffer_struct buffer_t; -static hid_t *first_hid = NULL; -static hid_t *last_hid = NULL; -// Make sure we use the correct runloop -CFRunLoopRef the_correct_runloop = NULL; -struct hid_struct { - IOHIDDeviceRef ref; - int open; - uint8_t buffer[BUFFER_SIZE]; - buffer_t *first_buffer; - buffer_t *last_buffer; - struct hid_struct *prev; - struct hid_struct *next; -}; -struct buffer_struct { - struct buffer_struct *next; - uint32_t len; - uint8_t buf[BUFFER_SIZE]; -}; - -static void add_hid(hid_t *); -static hid_t * get_hid(int); -static void free_all_hid(void); -static void hid_close(hid_t *); -static void attach_callback(void *, IOReturn, void *, IOHIDDeviceRef); -static void detach_callback(void *, IOReturn, void *hid_mgr, IOHIDDeviceRef dev); -static void input_callback(void *, IOReturn, void *, IOHIDReportType, uint32_t, uint8_t *, CFIndex); -//static void output_callback(hid_t *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len); -static void timeout_callback(CFRunLoopTimerRef, void *); - - -pjrc_rawhid::pjrc_rawhid() -{ - first_hid = NULL; - last_hid = NULL; + m_writeMutex = new QMutex(); + m_readMutex = new QMutex(); } pjrc_rawhid::~pjrc_rawhid() { + if (device_open) { + close(0); + } + + if (m_writeMutex) { + delete m_writeMutex; + m_writeMutex = NULL; + } + + if (m_readMutex) { + delete m_readMutex; + m_readMutex = NULL; + } } -// open - open 1 or more devices -// -// Inputs: -// max = maximum number of devices to open -// vid = Vendor ID, or -1 if any -// pid = Product ID, or -1 if any -// usage_page = top level usage page, or -1 if any -// usage = top level usage number, or -1 if any -// Output: -// actual number of devices opened -// +/** + * @brief open - open 1 or more devices + * @param[in] max maximum number of devices to open + * @param[in] vid Vendor ID, or -1 if any + * @param[in] pid Product ID, or -1 if any + * @param[in] usage_page top level usage page, or -1 if any + * @param[in] usage top level usage number, or -1 if any + * @returns actual number of devices opened + */ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) { - static IOHIDManagerRef hid_manager=NULL; CFMutableDictionaryRef dict; CFNumberRef num; IOReturn ret; - hid_t *p; - int count=0; - if (first_hid) free_all_hid(); - //printf("pjrc_rawhid_open, max=%d\n", max); - if (max < 1) return 0; + Q_ASSERT(hid_manager == NULL); + Q_ASSERT(device_open == false); + + attach_count = 0; + // Start the HID Manager - // http://developer.apple.com/technotes/tn2007/tn2187.html - if (!hid_manager) { - hid_manager = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); - if (hid_manager == NULL || CFGetTypeID(hid_manager) != IOHIDManagerGetTypeID()) { - if (hid_manager) CFRelease(hid_manager); - return 0; - } + hid_manager = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); + if (hid_manager == NULL || CFGetTypeID(hid_manager) != IOHIDManagerGetTypeID()) { + if (hid_manager) CFRelease(hid_manager); + return 0; } + if (vid > 0 || pid > 0 || usage_page > 0 || usage > 0) { // Tell the HID Manager what type of devices we want dict = CFDictionaryCreateMutable(kCFAllocatorDefault, 0, @@ -166,165 +131,147 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) } else { IOHIDManagerSetDeviceMatching(hid_manager, NULL); } + + // Set the run loop reference before configuring the attach callback + the_correct_runloop = CFRunLoopGetCurrent(); + // set up a callbacks for device attach & detach IOHIDManagerScheduleWithRunLoop(hid_manager, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); - IOHIDManagerRegisterDeviceMatchingCallback(hid_manager, attach_callback, NULL); - IOHIDManagerRegisterDeviceRemovalCallback(hid_manager, detach_callback, NULL); + IOHIDManagerRegisterDeviceMatchingCallback(hid_manager, pjrc_rawhid::attach_callback, this); + IOHIDManagerRegisterDeviceRemovalCallback(hid_manager, pjrc_rawhid::dettach_callback, this); ret = IOHIDManagerOpen(hid_manager, kIOHIDOptionsTypeNone); if (ret != kIOReturnSuccess) { - qDebug()<< "Could not start IOHIDManager"; IOHIDManagerUnscheduleFromRunLoop(hid_manager, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); CFRelease(hid_manager); return 0; } - // Set the run loop reference: - the_correct_runloop = CFRunLoopGetCurrent(); - qDebug() << "run loop"; + // let it do the callback for all devices while (CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0, true) == kCFRunLoopRunHandledSource) ; + // count up how many were added by the callback - for (p = first_hid; p; p = p->next) count++; - return count; + return attach_count; } -// recveive - receive a packet -// Inputs: -// num = device to receive from (zero based) -// buf = buffer to receive packet -// len = buffer's size -// timeout = time to wait, in milliseconds -// Output: -// number of bytes received, or -1 on error -// -int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) +/** + * @brief receive - receive a packet + * @param[in] num device to receive from (unused now) + * @param[in] buf buffer to receive packet + * @param[in] len buffer's size + * @param[in] timeout = time to wait, in milliseconds + * @returns number of bytes received, or -1 on error + */ +int pjrc_rawhid::receive(int, void *buf, int len, int timeout) { - hid_t *hid; - buffer_t *b; - CFRunLoopTimerRef timer=NULL; - CFRunLoopTimerContext context; - int ret=0, timeout_occurred=0; + QMutexLocker locker(m_readMutex); + Q_UNUSED(locker); - if (len < 1) return 0; - hid = get_hid(num); - if (!hid || !hid->open) return -1; - if ((b = hid->first_buffer) != NULL) { - if (len > b->len) len = b->len; - memcpy(buf, b->buf, len); - hid->first_buffer = b->next; - free(b); - return len; - } - memset(&context, 0, sizeof(context)); - context.info = &timeout_occurred; - timer = CFRunLoopTimerCreate(NULL, CFAbsoluteTimeGetCurrent() + - (double)timeout / 1000.0, 0, 0, 0, timeout_callback, &context); - CFRunLoopAddTimer(CFRunLoopGetCurrent(), timer, kCFRunLoopDefaultMode); - the_correct_runloop = CFRunLoopGetCurrent(); - //qDebug("--"); - while (1) { - //qDebug("."); - CFRunLoopRun(); // Found the problem: somehow the input_callback does not - // stop this CFRunLoopRun because it is hooked to a different run loop !!! - // Hence the use of the "correct_runloop" variable above. - //qDebug(" .."); + if (!device_open) + return -1; - if ((b = hid->first_buffer) != NULL) { - if (len > b->len) len = b->len; - memcpy(buf, b->buf, len); - hid->first_buffer = b->next; - free(b); - ret = len; - //qDebug("*************"); - break; - } - if (!hid->open) { - qDebug() << "pjrc_rawhid_recv, device not open\n"; - ret = -1; - break; - } - if (timeout_occurred) + // Pass information to the callback to stop this run loop and signal if a timeout occurred + struct timeout_info info; + info.loopRef = CFRunLoopGetCurrent();; + info.timed_out = false; + CFRunLoopTimerContext context; + memset(&context, 0, sizeof(context)); + context.info = &info; + + // Set up the timer for the timeout + CFRunLoopTimerRef timer; + timer = CFRunLoopTimerCreate(NULL, CFAbsoluteTimeGetCurrent() + (double)timeout / 1000.0, 0, 0, 0, timeout_callback, &context); + CFRunLoopAddTimer(CFRunLoopGetCurrent(), timer, kCFRunLoopDefaultMode); + + received_runloop = CFRunLoopGetCurrent(); + + // Run the CFRunLoop until either a timeout or data is available + while(1) { + if (buffer_count != 0) { + if (len > buffer_count) len = buffer_count; + memcpy(buf, buffer, len); + buffer_count = 0; break; - } - CFRunLoopTimerInvalidate(timer); - CFRelease(timer); - return ret; -} - -// send - send a packet -// Inputs: -// num = device to transmit to (zero based) -// buf = buffer containing packet to send -// len = number of bytes to transmit -// timeout = time to wait, in milliseconds -// Output: -// number of bytes sent, or -1 on error -// -int pjrc_rawhid::send(int num, void *buf, int len, int timeout) -{ - Q_UNUSED(timeout); - - hid_t *hid; - int result=-100; - - hid = get_hid(num); - if (!hid || !hid->open) return -1; -#if 1 -#warning "Send timeout not implemented on MACOSX" - uint8_t *report_buf = (uint8_t *) malloc(len); - memcpy(&report_buf[0], buf,len); - // Note: packet processing done in OS indepdent code - IOReturn ret = IOHIDDeviceSetReport(hid->ref, kIOHIDReportTypeOutput, 2, (uint8_t *)report_buf, len); - result = (ret == kIOReturnSuccess) ? len : -1; - if (err_get_system(ret) == err_get_system(sys_iokit)) - { - - // The error was in the I/O Kit system - UInt32 codeValue = err_get_code(ret); - qDebug("Returned: %x", codeValue); - // Can now perform test on error code, display it to user, or whatever. - usleep(1000000); - } - -#endif -#if 0 -#define TIMEOUT_FIXED - // No matter what I tried this never actually sends an output - // report and output_callback never gets called. Why?? - // Did I miss something? This is exactly the same params as - // the sync call that works. Is it an Apple bug? - // (submitted to Apple on 22-sep-2009, problem ID 7245050) - // - IOHIDDeviceScheduleWithRunLoop(hid->ref, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); - // should already be scheduled with run loop by attach_callback, - // sadly this doesn't make any difference either way - // - IOHIDDeviceSetReportWithCallback(hid->ref, kIOHIDReportTypeOutput, - 0, buf, len, (double)timeout / 1000.0, output_callback, &result); - while (1) { - printf("enter run loop (send)\n"); - CFRunLoopRun(); - printf("leave run loop (send)\n"); - if (result > -100) break; - if (!hid->open) { - result = -1; + } else if (info.timed_out) { + len = 0; break; } + CFRunLoopRun(); // Wait for data } -#endif - return result; + + CFRunLoopTimerInvalidate(timer); + CFRelease(timer); + + received_runloop = NULL; + + return len; } +/** + * @brief Helper class that will workaround the fact + * that the HID send is broken on OSX + */ +class Sender : public QThread +{ +public: + Sender(IOHIDDeviceRef d, uint8_t * b, int l) : + dev(d), buf(b), len(l), result(-1) { } + + void run() { + ret = IOHIDDeviceSetReport(dev, kIOHIDReportTypeOutput, 2, buf, len); + result = (ret == kIOReturnSuccess) ? len : -1; + } + + int result; + IOReturn ret; +private: + IOHIDDeviceRef dev; + uint8_t * buf; + int len; +}; + +/** + * @brief send - send a packet + * @param[in] num device to transmit to (zero based) + * @param[in] buf buffer containing packet to send + * @param[in] len number of bytes to transmit + * @param[in] timeout = time to wait, in milliseconds + * @returns number of bytes sent, or -1 on error + */ +int pjrc_rawhid::send(int, void *buf, int len, int timeout) +{ + // This lock ensures that when closing we don't do it until the + // write has terminated (and then the device_open flag is set to false) + QMutexLocker locker(m_writeMutex); + Q_UNUSED(locker); + + if(!device_open || unplugged) { + return -1; + } + + uint8_t *report_buf = (uint8_t *) malloc(len); + memcpy(&report_buf[0], buf,len); + + QEventLoop el; + Sender sender(dev, report_buf, len); + connect(&sender, SIGNAL(finished()), &el, SLOT(quit())); + sender.start(); + QTimer::singleShot(timeout, &el, SLOT(quit())); + el.exec(); + + return sender.result; +} + +//! Get the serial number for a HID device QString pjrc_rawhid::getserial(int num) { - hid_t *hid; -// char buf[128]; + QMutexLocker locker(m_readMutex); + Q_UNUSED(locker); - hid = get_hid(num); + if (!device_open || unplugged) + return ""; - if (!hid || !hid->open) return QString("Error"); - - CFTypeRef serialnum = IOHIDDeviceGetProperty(hid->ref, CFSTR(kIOHIDSerialNumberKey)); + CFTypeRef serialnum = IOHIDDeviceGetProperty(dev, CFSTR(kIOHIDSerialNumberKey)); if(serialnum && CFGetTypeID(serialnum) == CFStringGetTypeID()) { //Note: I'm not sure it will always succeed if encoded as MacRoman but that @@ -337,169 +284,106 @@ QString pjrc_rawhid::getserial(int num) { return QString("Error"); } -// close - close a device -// -// Inputs: -// num = device to close (zero based) -// Output -// (nothing) -// -void pjrc_rawhid::close(int num) +//! Close the HID device +void pjrc_rawhid::close(int) { - hid_t *hid; + // Make sure any pending locks are done + QMutexLocker lock(m_writeMutex); - hid = get_hid(num); - if (!hid || !hid->open) return; - hid_close(hid); - hid->open = 0; + if (device_open) { + device_open = false; + CFRunLoopStop(the_correct_runloop); + + if (!unplugged) { + IOHIDDeviceUnscheduleFromRunLoop(dev, the_correct_runloop, kCFRunLoopDefaultMode); + IOHIDDeviceRegisterInputReportCallback(dev, buffer, sizeof(buffer), NULL, NULL); + IOHIDDeviceClose(dev, kIOHIDOptionsTypeNone); + } + + IOHIDManagerRegisterDeviceRemovalCallback(hid_manager, NULL, NULL); + IOHIDManagerClose(hid_manager, 0); + + dev = NULL; + hid_manager = NULL; + } } -// -// -// Private Functions -// -// -static void input_callback(void *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) +/** + * @brief input Called to add input data to the buffer + * @param[in] id Report id + * @param[in] data The data buffer + * @param[in] len The report length + */ +void pjrc_rawhid::input(uint8_t *data, CFIndex len) { - Q_UNUSED(type); - Q_UNUSED(id); + if (!device_open) + return; - buffer_t *n; - hid_t *hid; - - //qDebug("input_callback, ret: %i - report id: %i buf: %x %x, len: %d\n", ret, id, data[0], data[1], len); - if (ret != kIOReturnSuccess || len < 1) return; - hid = (hid_t*)context; - if (!hid || hid->ref != sender) return; - qDebug() << "Processing packet"; - n = (buffer_t *)malloc(sizeof(buffer_t)); - if (!n) return; if (len > BUFFER_SIZE) len = BUFFER_SIZE; // Note: packet preprocessing done in OS independent code - memcpy(n->buf, &data[0], len); - n->len = len; - n->next = NULL; - if (!hid->first_buffer || !hid->last_buffer) { - hid->first_buffer = hid->last_buffer = n; - } else { - hid->last_buffer->next = n; - hid->last_buffer = n; - } - //qDebug() << "Stop CFRunLoop from input_callback" << CFRunLoopGetCurrent(); - CFRunLoopStop(the_correct_runloop); + memcpy(buffer, &data[0], len); + buffer_count = len; + + if (received_runloop) + CFRunLoopStop(received_runloop); } -static void timeout_callback(CFRunLoopTimerRef timer, void *info) +//! Callback for the HID driver on an input report +void pjrc_rawhid::input_callback(void *c, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) { - Q_UNUSED(timer); + if (ret != kIOReturnSuccess || len < 1) return; - //qDebug("timeout_callback\n"); - *(int *)info = 1; - //qDebug() << "Stop CFRunLoop from timeout_callback" << CFRunLoopGetCurrent(); - CFRunLoopStop(CFRunLoopGetCurrent()); + pjrc_rawhid *context = (pjrc_rawhid *) c; + context->input(data, len); } -static void add_hid(hid_t *h) +//! Timeout used for the +void pjrc_rawhid::timeout_callback(CFRunLoopTimerRef, void *i) { - if (!first_hid || !last_hid) { - first_hid = last_hid = h; - h->next = h->prev = NULL; - return; - } - last_hid->next = h; - h->prev = last_hid; - h->next = NULL; - last_hid = h; + struct timeout_info *info = (struct timeout_info *) i; + info->timed_out = true; + CFRunLoopStop(info->loopRef); } - -static hid_t * get_hid(int num) +//! Called on a dettach event +void pjrc_rawhid::dettach(IOHIDDeviceRef d) { - hid_t *p; - for (p = first_hid; p && num > 0; p = p->next, num--) ; - return p; + unplugged = true; + if (d == dev) + emit deviceUnplugged(0); } - -static void free_all_hid(void) +//! Called from the USB system and forwarded to the instance (context) +void pjrc_rawhid::dettach_callback(void *context, IOReturn, void *, IOHIDDeviceRef dev) { - hid_t *p, *q; - - for (p = first_hid; p; p = p->next) { - hid_close(p); - } - p = first_hid; - while (p) { - q = p; - p = p->next; - free(q); - } - first_hid = last_hid = NULL; + pjrc_rawhid *p = (pjrc_rawhid*) context; + p->dettach(dev); } - -static void hid_close(hid_t *hid) +/** + * @brief Called by the USB system + * @param dev The device that was attached + */ +void pjrc_rawhid::attach(IOHIDDeviceRef d) { - if (!hid || !hid->open || !hid->ref) return; - IOHIDDeviceUnscheduleFromRunLoop(hid->ref, CFRunLoopGetCurrent( ), kCFRunLoopDefaultMode); - IOHIDDeviceClose(hid->ref, kIOHIDOptionsTypeNone); - hid->ref = NULL; -} + // Store the device handle + dev = d; -static void detach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) -{ - Q_UNUSED(context); - Q_UNUSED(r); - Q_UNUSED(hid_mgr); - - hid_t *p; - - qDebug()<< "detach callback"; - for (p = first_hid; p; p = p->next) { - if (p->ref == dev) { - p->open = 0; - CFRunLoopStop(CFRunLoopGetCurrent()); - return; - } - } -} - -static void attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) -{ - Q_UNUSED(context); - Q_UNUSED(r); - Q_UNUSED(hid_mgr); - - struct hid_struct *h; - - qDebug() << "attach callback"; if (IOHIDDeviceOpen(dev, kIOHIDOptionsTypeNone) != kIOReturnSuccess) return; - h = (hid_t *)malloc(sizeof(hid_t)); - if (!h) return; - memset(h, 0, sizeof(hid_t)); - IOHIDDeviceScheduleWithRunLoop(dev, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); - IOHIDDeviceRegisterInputReportCallback(dev, h->buffer, sizeof(h->buffer), input_callback, h); - h->ref = dev; - h->open = 1; - add_hid(h); + // Disconnect the attach callback since we don't want to automatically reconnect + IOHIDManagerRegisterDeviceMatchingCallback(hid_manager, NULL, NULL); + IOHIDDeviceScheduleWithRunLoop(dev, the_correct_runloop, kCFRunLoopDefaultMode); + IOHIDDeviceRegisterInputReportCallback(dev, buffer, sizeof(buffer), pjrc_rawhid::input_callback, this); + + attach_count++; + device_open = true; + unplugged = false; } -#ifdef TIMEOUT_FIXED -static void output_callback(hid_t *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) +//! Called from the USB system and forwarded to the instance (context) +void pjrc_rawhid::attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) { - Q_UNUSED(sender); - Q_UNUSED(type); - Q_UNUSED(id); - Q_UNUSED(data); - - qDebug()<< QString("output_callback, r=%1").arg(ret); -// printf("output_callback, r=%d\n", ret); - if (ret == kIOReturnSuccess) { - *(int *)context = len; - } else { - // timeout if not success? - *(int *)context = 0; - } - CFRunLoopStop(CFRunLoopGetCurrent()); + pjrc_rawhid *p = (pjrc_rawhid*) context; + p->attach(dev); } -#endif + diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp b/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp index 4649d6162..3a1aa896d 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp @@ -138,19 +138,20 @@ RawHIDReadThread::RawHIDReadThread(RawHID *hid) hidno(hid->m_deviceNo), m_running(true) { + hid->m_startedMutex->lock(); } RawHIDReadThread::~RawHIDReadThread() { m_running = false; //wait for the thread to terminate - if(wait(1000) == false) + if(wait(10000) == false) qDebug() << "Cannot terminate RawHIDReadThread"; } void RawHIDReadThread::run() { - qDebug() << "Read thread started"; + m_running = m_hid->openDevice(); while(m_running) { //here we use a temporary buffer so we don't need to lock @@ -182,6 +183,7 @@ void RawHIDReadThread::run() m_running=false; } } + m_hid->closeDevice(); } int RawHIDReadThread::getReadData(char *data, int size) @@ -216,13 +218,12 @@ RawHIDWriteThread::~RawHIDWriteThread() { m_running = false; //wait for the thread to terminate - if(wait(1000) == false) + if(wait(10000) == false) qDebug() << "Cannot terminate RawHIDReadThread"; } void RawHIDWriteThread::run() { - qDebug() << "Write thread started"; while(m_running) { char buffer[WRITE_SIZE] = {0}; @@ -300,54 +301,66 @@ RawHID::RawHID(const QString &deviceName) m_mutex(NULL) { - m_mutex = new QMutex(QMutex::Recursive); + m_mutex = new QMutex(QMutex::Recursive); + m_startedMutex = new QMutex(); - // detect if the USB device is unplugged - QObject::connect(&dev, SIGNAL(deviceUnplugged(int)), this, SLOT(onDeviceUnplugged(int))); + // detect if the USB device is unplugged + QObject::connect(&dev, SIGNAL(deviceUnplugged(int)), this, SLOT(onDeviceUnplugged(int))); - int opened = dev.open(USB_MAX_DEVICES, USBMonitor::idVendor_OpenPilot, -1, USB_USAGE_PAGE, USB_USAGE); - for (int i =0; i< opened; i++) { - if (deviceName == dev.getserial(i)) - m_deviceNo = i; - else - dev.close(i); - } - - /* - // TODO: NOT WORKING FOR MULTIPLE DEVICES with the same PID! - QList devices = USBMonitor::instance()->availableDevices(USBMonitor::idVendor_OpenPilot,-1,-1,USBMonitor::Running); - foreach( USBPortInfo device, devices) { - if (deviceName == device.serialNumber) { - opened = dev.open(1,device.vendorID, device.productID,USB_USAGE_PAGE,USB_USAGE); - break; - } - } - */ - - //didn't find the device we are trying to open (shouldnt happen) - if (opened < 0) - { - qDebug() << "Error: cannot open device " << deviceName; - return; - } - - //m_deviceNo = 0; - m_readThread = new RawHIDReadThread(this); m_writeThread = new RawHIDWriteThread(this); + // Starting the read thread will lock the m_startexMutex until the + // device is opened (which happens in that thread). + m_readThread = new RawHIDReadThread(this); m_readThread->start(); + + m_startedMutex->lock(); +} + +/** + * @brief RawHID::openDevice This method opens the USB connection + * It is uses as a callback from the read thread so that the USB + * system code is registered in that thread instead of the calling + * thread (usually UI) + */ +bool RawHID::openDevice() { + int opened = dev.open(USB_MAX_DEVICES, USBMonitor::idVendor_OpenPilot, -1, USB_USAGE_PAGE, USB_USAGE); + for (int i =0; i< opened; i++) { + if (serialNumber == dev.getserial(i)) + m_deviceNo = i; + else + dev.close(i); + } + + // Now things are opened or not (from read thread) allow the constructor to complete + m_startedMutex->unlock(); + + //didn't find the device we are trying to open (shouldnt happen) + device_open = opened >= 0; + if (opened < 0) + { + return false; + } + m_writeThread->start(); + + return true; +} + +/** + * @brief RawHID::closeDevice This method closes the USB connection + * It is uses as a callback from the read thread so that the USB + * system code is unregistered from that thread\ + */ +bool RawHID::closeDevice() { + dev.close(m_deviceNo); } RawHID::~RawHID() { - dev.close(m_deviceNo); - - if (m_readThread) - delete m_readThread; - - if (m_writeThread) - delete m_writeThread; + // If the read thread exists then the device is open + if (m_readThread) + close(); } void RawHID::onDeviceUnplugged(int num) @@ -356,7 +369,6 @@ void RawHID::onDeviceUnplugged(int num) return; // the USB device has been unplugged - close(); } @@ -369,43 +381,38 @@ bool RawHID::open(OpenMode mode) QIODevice::open(mode); - if (!m_readThread) - m_readThread = new RawHIDReadThread(this); - - if (!m_writeThread) - m_writeThread = new RawHIDWriteThread(this); - - if (m_readThread) m_readThread->start(); // Pip - if (m_writeThread) m_writeThread->start(); // Pip + Q_ASSERT(m_readThread); + Q_ASSERT(m_writeThread); + if (m_readThread) m_readThread->start(); + if (m_writeThread) m_writeThread->start(); return true; } void RawHID::close() { - emit aboutToClose(); + qDebug() << "RawHID::close()"; + emit aboutToClose(); + if (m_writeThread) + { + qDebug() << "About to terminate write thread"; + m_writeThread->terminate(); + delete m_writeThread; + m_writeThread = NULL; + qDebug() << "Write thread terminated"; + } - m_mutex->lock(); - if (m_readThread) - { - m_readThread->terminate(); - delete m_readThread; // calls wait - m_readThread = NULL; - } + if (m_readThread) + { + qDebug() << "About to terminate read thread"; + m_readThread->terminate(); + delete m_readThread; // calls wait + m_readThread = NULL; + qDebug() << "Read thread terminated"; + } - if (m_writeThread) - { - m_writeThread->terminate(); - delete m_writeThread; - m_writeThread = NULL; - } - - dev.close(m_deviceNo); - - m_mutex->unlock(); - - emit closed(); + emit closed(); QIODevice::close(); } diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid.h b/ground/openpilotgcs/src/plugins/rawhid/rawhid.h index 12ec78384..f3487bf20 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid.h @@ -74,15 +74,23 @@ protected: virtual qint64 bytesAvailable() const; virtual qint64 bytesToWrite() const; + //! Callback from the read thread to open the device + bool openDevice(); + + //! Callback from teh read thread to close the device + bool closeDevice(); + QString serialNumber; int m_deviceNo; pjrc_rawhid dev; + bool device_open; RawHIDReadThread *m_readThread; RawHIDWriteThread *m_writeThread; QMutex *m_mutex; + QMutex *m_startedMutex; }; #endif // RAWHID_H diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp index d3be05e8d..24276b981 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp @@ -72,6 +72,7 @@ void RawHIDConnection::onDeviceConnected() */ void RawHIDConnection::onDeviceDisconnected() { + qDebug() << "onDeviceDisconnected()"; if (enablePolling) emit availableDevChanged(this); } @@ -110,15 +111,13 @@ QIODevice *RawHIDConnection::openDevice(const QString &deviceName) void RawHIDConnection::closeDevice(const QString &deviceName) { Q_UNUSED(deviceName); - //added by andrew... if (RawHidHandle) { + qDebug() << "Closing the device here"; RawHidHandle->close(); - delete RawHidHandle; RawHidHandle = NULL; } - //end added by andrew } QString RawHIDConnection::connectionName() diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h index ee9aa49aa..324471a14 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h @@ -31,7 +31,6 @@ #include "rawhid_global.h" #include "rawhid.h" #include "usbmonitor.h" -#include "usbsignalfilter.h" #include "coreplugin/iconnection.h" #include diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp index 24d5a578e..1a0c06457 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp @@ -40,7 +40,7 @@ ControllerPage::ControllerPage(SetupWizard *wizard, QWidget *parent) : m_connectionManager = getWizard()->getConnectionManager(); Q_ASSERT(m_connectionManager); - connect(m_connectionManager, SIGNAL(availableDevicesChanged(QLinkedList)), this, SLOT(devicesChanged(QLinkedList))); + connect(m_connectionManager, SIGNAL(availableDevicesChanged(QLinkedList)), this, SLOT(devicesChanged(QLinkedList))); connect(m_connectionManager, SIGNAL(deviceConnected(QIODevice*)), this, SLOT(connectionStatusChanged())); connect(m_connectionManager, SIGNAL(deviceDisconnected()), this, SLOT(connectionStatusChanged())); @@ -132,7 +132,7 @@ void ControllerPage::setControllerType(SetupWizard::CONTROLLER_TYPE type) } } -void ControllerPage::devicesChanged(QLinkedList devices) +void ControllerPage::devicesChanged(QLinkedList devices) { // Get the selected item before the update if any QString currSelectedDeviceName = ui->deviceCombo->currentIndex() != -1 ? @@ -145,10 +145,10 @@ void ControllerPage::devicesChanged(QLinkedList devices) int i = 0; // Loop and fill the combo with items from connectionmanager - foreach (Core::devListItem device, devices) + foreach (Core::DevListItem deviceItem, devices) { - ui->deviceCombo->addItem(device.displayName); - QString deviceName = (const QString)device.devName; + ui->deviceCombo->addItem(deviceItem.getConName()); + QString deviceName = (const QString)deviceItem.getConName(); ui->deviceCombo->setItemData(ui->deviceCombo->count() - 1, deviceName, Qt::ToolTipRole); if(currSelectedDeviceName != "" && currSelectedDeviceName == deviceName) { indexOfSelectedItem = i; @@ -169,7 +169,7 @@ void ControllerPage::connectionStatusChanged() ui->deviceCombo->setEnabled(false); ui->connectButton->setText(tr("Disconnect")); ui->boardTypeCombo->setEnabled(false); - QString connectedDeviceName = m_connectionManager->getCurrentDevice().devName; + QString connectedDeviceName = m_connectionManager->getCurrentDevice().getConName(); for(int i = 0; i < ui->deviceCombo->count(); ++i) { if(connectedDeviceName == ui->deviceCombo->itemData(i, Qt::ToolTipRole).toString()) { ui->deviceCombo->setCurrentIndex(i); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h index 177976731..c906fff5f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h @@ -58,7 +58,7 @@ private: Core::ConnectionManager *m_connectionManager; private slots: - void devicesChanged(QLinkedList devices); + void devicesChanged(QLinkedList devices); void connectionStatusChanged(); void connectDisconnect(); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 9e7a27e5d..65fc12a24 100755 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -63,6 +63,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/velocityactual.h \ $$UAVOBJECT_SYNTHETICS/guidancesettings.h \ $$UAVOBJECT_SYNTHETICS/positiondesired.h \ + $$UAVOBJECT_SYNTHETICS/relaytuning.h \ + $$UAVOBJECT_SYNTHETICS/relaytuningsettings.h \ $$UAVOBJECT_SYNTHETICS/ratedesired.h \ $$UAVOBJECT_SYNTHETICS/firmwareiapobj.h \ $$UAVOBJECT_SYNTHETICS/i2cstats.h \ @@ -128,6 +130,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/velocityactual.cpp \ $$UAVOBJECT_SYNTHETICS/guidancesettings.cpp \ $$UAVOBJECT_SYNTHETICS/positiondesired.cpp \ + $$UAVOBJECT_SYNTHETICS/relaytuningsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/relaytuning.cpp \ $$UAVOBJECT_SYNTHETICS/ratedesired.cpp \ $$UAVOBJECT_SYNTHETICS/firmwareiapobj.cpp \ $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \ diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index dc08d94f2..810ddfa06 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -87,11 +87,13 @@ DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): m_eventloop.exec(); QList devices; devices = USBMonitor::instance()->availableDevices(0x20a0,-1,-1,USBMonitor::Bootloader); - if (devices.length()==1 && hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) { - qDebug()<<"OP_DFU detected first time"; - mready=true; - QTimer::singleShot(200,&m_eventloop, SLOT(quit())); - m_eventloop.exec(); + if (devices.length()==1) { + if (hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) { + mready=true; + QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + m_eventloop.exec(); + } else + hidHandle.close(0); } else { // Wait for the board to appear on the USB bus: USBSignalFilter filter(0x20a0,-1,-1,USBMonitor::Bootloader); @@ -106,17 +108,20 @@ DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): QTimer::singleShot(2000,&m_eventloop, SLOT(quit())); m_eventloop.exec(); devices = USBMonitor::instance()->availableDevices(0x20a0,-1,-1,USBMonitor::Bootloader); + qDebug() << "Devices length: " << devices.length(); if (devices.length()==1) { - if(hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) + qDebug() << "Opening device"; + if(hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) { - QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + QTimer::singleShot(200,&m_eventloop, SLOT(quit())); m_eventloop.exec(); qDebug()<<"OP_DFU detected after delay"; mready=true; + qDebug() << "Detected"; break; - } - } - else { + } else + hidHandle.close(0); + } else { qDebug() << devices.length() << " device(s) detected, don't know what to do!"; mready = false; } @@ -171,7 +176,6 @@ bool DFUObject::enterDFU(int const &devNumber) buf[9] = 1; //DFU Data3 int result = sendData(buf, BUF_LEN); - // int result = hidHandle.send(0,buf, BUF_LEN, 500); if(result<1) return false; if(debug) @@ -216,7 +220,6 @@ bool DFUObject::StartUpload(qint32 const & numberOfBytes, TransferTypes const & int result = sendData(buf, BUF_LEN); delay::msleep(1000); - // int result = hidHandle.send(0,buf, BUF_LEN, 5000); if(debug) qDebug() << result << " bytes sent"; @@ -441,7 +444,6 @@ bool DFUObject::StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, Tra buf[9] = 1; //DFU Data3 int result = sendData(buf, BUF_LEN); - //int result = hidHandle.send(0,buf, BUF_LEN, 500); if(debug) qDebug() << "StartDownload:"<0) diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index a692433b1..39837e3de 100755 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -249,8 +249,8 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) // The board is now reset: we have to disconnect telemetry Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager(); - QString dli = cm->getCurrentDevice().Name; - QString dlj = cm->getCurrentDevice().devName; + QString dli = cm->getCurrentDevice().getConName(); + QString dlj = cm->getCurrentDevice().getConName(); cm->disconnectDevice(); QTimer::singleShot(200, &m_eventloop, SLOT(quit())); m_eventloop.exec(); @@ -378,7 +378,8 @@ void UploaderGadgetWidget::systemSafeBoot() } /** - Tells the system to boot (from Bootloader state) + * Tells the system to boot (from Bootloader state) + * @param[in] safeboot Indicates whether the firmware should use the stock HWSettings */ void UploaderGadgetWidget::commonSystemBoot(bool safeboot) { @@ -456,7 +457,7 @@ void UploaderGadgetWidget::systemRescue() delete dfu; dfu = NULL; } - // Avoid dumb users pressing Rescue twice. It can happen. + // Avoid users pressing Rescue twice. m_config->rescueButton->setEnabled(false); // Now we're good to go: @@ -523,25 +524,6 @@ void UploaderGadgetWidget::systemRescue() m_config->rescueButton->setEnabled(true); return; } - if ((eBoardCC != dfu->GetBoardType(0)) && (QMessageBox::question(this,tr("OpenPilot Uploader"),tr("If you want to search for other boards connect power now and press Yes"),QMessageBox::Yes,QMessageBox::No)==QMessageBox::Yes)) - { - log("\nWaiting..."); - QTimer::singleShot(3000, &m_eventloop, SLOT(quit())); - m_eventloop.exec(); - log("Detecting second board..."); - repaint(); - if(!dfu->findDevices()) - { - // We will only end up here in case somehow all the boards - // disappeared, including the one we detected earlier. - log("Could not detect any board, aborting!"); - delete dfu; - dfu = NULL; - cm->resumePolling(); - m_config->rescueButton->setEnabled(true); - return; - } - } log(QString("Found ") + QString::number(dfu->numberOfDevices) + QString(" device(s).")); if (dfu->numberOfDevices > 5) { log("Inconsistent number of devices, aborting!"); @@ -566,6 +548,7 @@ void UploaderGadgetWidget::systemRescue() m_config->rescueButton->setEnabled(false); currentStep = IAP_STATE_BOOTLOADER; // So that we can boot from the GUI afterwards. } + void UploaderGadgetWidget::perform() { if(m_progress->value()==19) diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 22c4f9360..46b4a299c 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -4,7 +4,7 @@ - + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 308cf01ac..b225c6715 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -18,7 +18,7 @@ - + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index d9c5af7a5..396dcfea1 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -18,13 +18,13 @@ - - - + + + - + diff --git a/shared/uavobjectdefinition/relaytuning.xml b/shared/uavobjectdefinition/relaytuning.xml new file mode 100644 index 000000000..d781e45ca --- /dev/null +++ b/shared/uavobjectdefinition/relaytuning.xml @@ -0,0 +1,11 @@ + + + The input to the relay tuning. + + + + + + + + diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml new file mode 100644 index 000000000..df28c8575 --- /dev/null +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -0,0 +1,15 @@ + + + Setting to run a relay tuning algorithm + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 02c63cd90..8c400e68d 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 7cd7de210..b1b541310 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -24,6 +24,8 @@ + + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 551167501..45b1a3b09 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -1,9 +1,9 @@ Task information - - - + + +