rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Merge branch 'next' into thread/OP-39

Browse Source 
Conflicts:
	ground/openpilotgcs/src/plugins/config/configgadgetwidget.h
	ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp
	ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h
This commit is contained in:
Fredrik Arvidsson 2012-09-13 22:11:37 +02:00
commit 4a09886e91
64 changed files with 3782 additions and 1438 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Makefile
View File

@ -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

3
androidgcs/src/org/openpilot/uavtalk/uavobjects/FlightStatus.java
View File

@ -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;

2
androidgcs/src/org/openpilot/uavtalk/uavobjects/GPSSettings.java
View File

@ -102,7 +102,7 @@ public class GPSSettings extends UAVDataObject {
*/
public void setDefaultFieldValues()
{
getField("DataProtocol").setValue("NMEA");
getField("DataProtocol").setValue("UBX");
}

7
androidgcs/src/org/openpilot/uavtalk/uavobjects/HwSettings.java
View File

@ -194,6 +194,7 @@ public class HwSettings extends UAVDataObject {
USB_HIDPortElemNames.add("0");
List<String> USB_HIDPortEnumOptions = new ArrayList<String>();
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<String> OptionalModulesEnumOptions = new ArrayList<String>();
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;

13
androidgcs/src/org/openpilot/uavtalk/uavobjects/ManualControlSettings.java
View File

@ -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<String> Stabilization2SettingsElemNames = new ArrayList<String>();
@ -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<String> Stabilization3SettingsElemNames = new ArrayList<String>();
@ -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<String> FlightModeNumberElemNames = new ArrayList<String>();
@ -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;

195
androidgcs/src/org/openpilot/uavtalk/uavobjects/PathAction.java Normal file
View File

@ -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<UAVObjectField> fields = new ArrayList<UAVObjectField>();
List<String> ModeParametersElemNames = new ArrayList<String>();
ModeParametersElemNames.add("0");
ModeParametersElemNames.add("1");
ModeParametersElemNames.add("2");
ModeParametersElemNames.add("3");
fields.add( new UAVObjectField("ModeParameters", "", UAVObjectField.FieldType.FLOAT32, ModeParametersElemNames, null) );
List<String> ConditionParametersElemNames = new ArrayList<String>();
ConditionParametersElemNames.add("0");
ConditionParametersElemNames.add("1");
ConditionParametersElemNames.add("2");
ConditionParametersElemNames.add("3");
fields.add( new UAVObjectField("ConditionParameters", "", UAVObjectField.FieldType.FLOAT32, ConditionParametersElemNames, null) );
List<String> ModeElemNames = new ArrayList<String>();
ModeElemNames.add("0");
List<String> ModeEnumOptions = new ArrayList<String>();
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<String> EndConditionElemNames = new ArrayList<String>();
EndConditionElemNames.add("0");
List<String> EndConditionEnumOptions = new ArrayList<String>();
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<String> CommandElemNames = new ArrayList<String>();
CommandElemNames.add("0");
List<String> CommandEnumOptions = new ArrayList<String>();
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<String> JumpDestinationElemNames = new ArrayList<String>();
JumpDestinationElemNames.add("0");
fields.add( new UAVObjectField("JumpDestination", "waypoint", UAVObjectField.FieldType.UINT8, JumpDestinationElemNames, null) );
List<String> ErrorDestinationElemNames = new ArrayList<String>();
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<UAVObjectField> 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;
}

146
androidgcs/src/org/openpilot/uavtalk/uavobjects/RelayTuning.java Normal file
View File

@ -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<UAVObjectField> fields = new ArrayList<UAVObjectField>();
List<String> PeriodElemNames = new ArrayList<String>();
PeriodElemNames.add("Roll");
PeriodElemNames.add("Pitch");
PeriodElemNames.add("Yaw");
fields.add( new UAVObjectField("Period", "ms", UAVObjectField.FieldType.FLOAT32, PeriodElemNames, null) );
List<String> GainElemNames = new ArrayList<String>();
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<UAVObjectField> 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;
}

171
androidgcs/src/org/openpilot/uavtalk/uavobjects/RelayTuningSettings.java Normal file
View File

@ -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<UAVObjectField> fields = new ArrayList<UAVObjectField>();
List<String> RateGainElemNames = new ArrayList<String>();
RateGainElemNames.add("0");
fields.add( new UAVObjectField("RateGain", "", UAVObjectField.FieldType.FLOAT32, RateGainElemNames, null) );
List<String> AttitudeGainElemNames = new ArrayList<String>();
AttitudeGainElemNames.add("0");
fields.add( new UAVObjectField("AttitudeGain", "", UAVObjectField.FieldType.FLOAT32, AttitudeGainElemNames, null) );
List<String> AmplitudeElemNames = new ArrayList<String>();
AmplitudeElemNames.add("0");
fields.add( new UAVObjectField("Amplitude", "", UAVObjectField.FieldType.FLOAT32, AmplitudeElemNames, null) );
List<String> HysteresisThreshElemNames = new ArrayList<String>();
HysteresisThreshElemNames.add("0");
fields.add( new UAVObjectField("HysteresisThresh", "deg/s", UAVObjectField.FieldType.UINT8, HysteresisThreshElemNames, null) );
List<String> ModeElemNames = new ArrayList<String>();
ModeElemNames.add("0");
List<String> ModeEnumOptions = new ArrayList<String>();
ModeEnumOptions.add("Rate");
ModeEnumOptions.add("Attitude");
fields.add( new UAVObjectField("Mode", "", UAVObjectField.FieldType.ENUM, ModeElemNames, ModeEnumOptions) );
List<String> BehaviorElemNames = new ArrayList<String>();
BehaviorElemNames.add("0");
List<String> BehaviorEnumOptions = new ArrayList<String>();
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<UAVObjectField> 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;
}

4
androidgcs/src/org/openpilot/uavtalk/uavobjects/StabilizationDesired.java
View File

@ -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;

12
androidgcs/src/org/openpilot/uavtalk/uavobjects/StabilizationSettings.java
View File

@ -131,6 +131,10 @@ public class StabilizationSettings extends UAVDataObject {
GyroTauElemNames.add("0");
fields.add( new UAVObjectField("GyroTau", "", UAVObjectField.FieldType.FLOAT32, GyroTauElemNames, null) );
List<String> DerivativeGammaElemNames = new ArrayList<String>();
DerivativeGammaElemNames.add("0");
fields.add( new UAVObjectField("DerivativeGamma", "", UAVObjectField.FieldType.FLOAT32, DerivativeGammaElemNames, null) );
List<String> WeakLevelingKpElemNames = new ArrayList<String>();
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<String> DerivativeCutoffElemNames = new ArrayList<String>();
DerivativeCutoffElemNames.add("0");
fields.add( new UAVObjectField("DerivativeCutoff", "Hz", UAVObjectField.FieldType.UINT8, DerivativeCutoffElemNames, null) );
List<String> MaxAxisLockElemNames = new ArrayList<String>();
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;

6
androidgcs/src/org/openpilot/uavtalk/uavobjects/SystemAlarms.java
View File

@ -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;

8
androidgcs/src/org/openpilot/uavtalk/uavobjects/TaskInfo.java
View File

@ -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<String> RunningElemNames = new ArrayList<String>();
@ -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<String> RunningEnumOptions = new ArrayList<String>();
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;

7
androidgcs/src/org/openpilot/uavtalk/uavobjects/UAVObjectsInitialize.java
View File

@ -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();

17
androidgcs/src/org/openpilot/uavtalk/uavobjects/Waypoint.java
View File

@ -58,23 +58,12 @@ public class Waypoint extends UAVDataObject {
fields.add( new UAVObjectField("Position", "m", UAVObjectField.FieldType.FLOAT32, PositionElemNames, null) );
List<String> VelocityElemNames = new ArrayList<String>();
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<String> YawDesiredElemNames = new ArrayList<String>();
YawDesiredElemNames.add("0");
fields.add( new UAVObjectField("YawDesired", "deg", UAVObjectField.FieldType.FLOAT32, YawDesiredElemNames, null) );
List<String> ActionElemNames = new ArrayList<String>();
ActionElemNames.add("0");
List<String> ActionEnumOptions = new ArrayList<String>();
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;

11
flight/CopterControl/Makefile
View File

@ -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)

158
flight/Libraries/math/pid.c Normal file
View File

@ -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;
}

52
flight/Libraries/math/pid.h Normal file
View File

@ -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 */

142
flight/Libraries/math/sin_lookup.c Normal file
View File

@ -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);
}

40
flight/Libraries/math/sin_lookup.h Normal file
View File

@ -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

333
flight/Modules/Autotune/autotune.c Normal file
View File

@ -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 <pios_board_info.h>
// 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;
}
}
/**
* @}
* @}
*/

37
flight/Modules/Autotune/inc/autotune.h Normal file
View File

@ -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

5
flight/Modules/ManualControl/inc/manualcontrol.h
View File

@ -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();

10
flight/Modules/ManualControl/manualcontrol.c
View File

@ -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;

39
flight/Modules/Stabilization/inc/relay_tuning.h Normal file
View File

@ -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

2
flight/Modules/Stabilization/inc/stabilization.h
View File

@ -33,6 +33,8 @@
#ifndef STABILIZATION_H
#define STABILIZATION_H
enum {ROLL,PITCH,YAW,MAX_AXES};
int32_t StabilizationInitialize();
#endif // STABILIZATION_H

42
flight/Modules/Stabilization/inc/virtualflybar.h Normal file
View File

@ -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 */

151
flight/Modules/Stabilization/relay_tuning.c Normal file
View File

@ -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;
}

430
flight/Modules/Stabilization/stabilization.c
View File

@ -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)
* @}
* @}
*/

119
flight/Modules/Stabilization/virtualflybar.c Normal file
View File

@ -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;
}

1
flight/PiOS/Boards/STM32103CB_CC_Rev1.h
View File

@ -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

2
flight/Revolution/UAVObjects.inc
View File

@ -62,6 +62,8 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += positiondesired
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += sonaraltitude
UAVOBJSRCFILENAMES += stabilizationdesired

15
ground/openpilotgcs/src/libs/qextserialport/src/qextserialenumerator_osx.cpp
View File

@ -22,8 +22,7 @@ QextSerialEnumerator::~QextSerialEnumerator( )
// static
QList<QextPortInfo> QextSerialEnumerator::getPorts()
{
QList<QextPortInfo> infoList;
{ QList<QextPortInfo> infoList;
io_iterator_t serialPortIterator = 0;
kern_return_t kernResult = KERN_FAILURE;
CFMutableDictionaryRef matchingDictionary;
@ -44,18 +43,6 @@ QList<QextPortInfo> 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;
}

1084
ground/openpilotgcs/src/plugins/config/autotune.ui Normal file
View File

@ -0,0 +1,1084 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>AutotuneWidget</class>
<widget class="QWidget" name="AutotuneWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>443</width>
<height>575</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<widget class="QGroupBox" name="groupBox">
<property name="geometry">
<rect>
<x>20</x>
<y>10</y>
<width>401</width>
<height>131</height>
</rect>
</property>
<property name="title">
<string>Tuning Aggressiveness</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>Rate Tuning:</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Attitude Tuning:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QSlider" name="rateTuning">
<property name="maximum">
<number>100</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuningSettings</string>
<string>fieldname:RateGain</string>
<string>scale:0.01</string>
<string>haslimits:no</string>
</stringlist>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QSlider" name="attitudeTuning">
<property name="maximum">
<number>100</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuningSettings</string>
<string>fieldname:AttitudeGain</string>
<string>scale:0.01</string>
<string>haslimits:no</string>
</stringlist>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QGroupBox" name="groupBox_2">
<property name="geometry">
<rect>
<x>20</x>
<y>250</y>
<width>401</width>
<height>121</height>
</rect>
</property>
<property name="title">
<string>Computed Values</string>
</property>
<layout class="QGridLayout" name="gridLayout_3">
<item row="0" column="1">
<widget class="QLabel" name="label_5">
<property name="text">
<string>RateKp</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Roll</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_9">
<property name="text">
<string>RateKi</string>
</property>
</widget>
</item>
<item row="0" column="3">
<widget class="QLabel" name="label_10">
<property name="text">
<string>AttitudeKp</string>
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QLabel" name="label_11">
<property name="text">
<string>AttitudeKi</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_12">
<property name="text">
<string>Pitch</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="rollRateKp">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="pitchRateKp">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="rollRateKi">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="pitchRateKi">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="1" column="3">
<widget class="QLabel" name="rollAttitudeKp">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="2" column="3">
<widget class="QLabel" name="pitchAttitudeKp">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="1" column="4">
<widget class="QLabel" name="rollAttitudeKi">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="2" column="4">
<widget class="QLabel" name="pitchAttitudeKi">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QGroupBox" name="groupBox_3">
<property name="geometry">
<rect>
<x>20</x>
<y>140</y>
<width>401</width>
<height>111</height>
</rect>
</property>
<property name="title">
<string>Measured Properties</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="1" column="0">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Roll:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="measuredRollPeriod">
<property name="text">
<string>0</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuning</string>
<string>fieldname:Period</string>
<string>element:Roll</string>
</stringlist>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="measuredRollGain">
<property name="text">
<string>0</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuning</string>
<string>fieldname:Gain</string>
<string>element:Roll</string>
</stringlist>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Period (ms)</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_7">
<property name="text">
<string>Gain (deg/s) / output</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_8">
<property name="text">
<string>Pitch</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="measuredPitchPeriod">
<property name="text">
<string>0</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuning</string>
<string>fieldname:Period</string>
<string>element:Pitch</string>
</stringlist>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="measuredPitchGain">
<property name="text">
<string>0</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuning</string>
<string>fieldname:Gain</string>
<string>element:Pitch</string>
</stringlist>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QGroupBox" name="RateStabilizationGroup_21">
<property name="geometry">
<rect>
<x>20</x>
<y>480</y>
<width>401</width>
<height>79</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>79</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>79</height>
</size>
</property>
<property name="palette">
<palette>
<active>
<colorrole role="WindowText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="Button">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Light">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="Midlight">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>251</red>
<green>251</green>
<blue>251</blue>
</color>
</brush>
</colorrole>
<colorrole role="Dark">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="Mid">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>165</red>
<green>165</green>
<blue>165</blue>
</color>
</brush>
</colorrole>
<colorrole role="Text">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="BrightText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="ButtonText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="Base">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Window">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Shadow">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="AlternateBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>251</red>
<green>251</green>
<blue>251</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>220</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
</active>
<inactive>
<colorrole role="WindowText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="Button">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Light">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="Midlight">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>251</red>
<green>251</green>
<blue>251</blue>
</color>
</brush>
</colorrole>
<colorrole role="Dark">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="Mid">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>165</red>
<green>165</green>
<blue>165</blue>
</color>
</brush>
</colorrole>
<colorrole role="Text">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="BrightText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="ButtonText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="Base">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Window">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Shadow">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="AlternateBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>251</red>
<green>251</green>
<blue>251</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>220</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
</inactive>
<disabled>
<colorrole role="WindowText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="Button">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Light">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="Midlight">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>251</red>
<green>251</green>
<blue>251</blue>
</color>
</brush>
</colorrole>
<colorrole role="Dark">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="Mid">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>165</red>
<green>165</green>
<blue>165</blue>
</color>
</brush>
</colorrole>
<colorrole role="Text">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="BrightText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="ButtonText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="Base">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Window">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Shadow">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="AlternateBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>248</red>
<green>248</green>
<blue>248</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>220</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
</disabled>
</palette>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="title">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<layout class="QGridLayout" name="gridLayout_10">
<item row="0" column="0">
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>111</width>
<height>10</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="1">
<widget class="QPushButton" name="stabilizationReloadBoardData_6">
<property name="minimumSize">
<size>
<width>120</width>
<height>28</height>
</size>
</property>
<property name="toolTip">
<string>Reloads the saved settings into GCS.
Useful if you have accidentally changed some settings.</string>
</property>
<property name="styleSheet">
<string notr="true">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;
}</string>
</property>
<property name="text">
<string>Reload Board Data</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>button:reload</string>
<string>buttongroup:10</string>
</stringlist>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QPushButton" name="saveStabilizationToRAM_6">
<property name="minimumSize">
<size>
<width>60</width>
<height>28</height>
</size>
</property>
<property name="toolTip">
<string>Send settings to the board but do not save to the non-volatile memory</string>
</property>
<property name="styleSheet">
<string notr="true">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;
}</string>
</property>
<property name="text">
<string>Apply</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>button:apply</string>
</stringlist>
</property>
</widget>
</item>
<item row="0" column="3">
<widget class="QPushButton" name="saveStabilizationToSD_6">
<property name="minimumSize">
<size>
<width>60</width>
<height>28</height>
</size>
</property>
<property name="toolTip">
<string>Send settings to the board and save to the non-volatile memory</string>
</property>
<property name="styleSheet">
<string notr="true">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;
}</string>
</property>
<property name="text">
<string>Save</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>button:save</string>
</stringlist>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QLabel" name="label_22">
<property name="geometry">
<rect>
<x>30</x>
<y>440</y>
<width>371</width>
<height>31</height>
</rect>
</property>
<property name="text">
<string>The buttons below change the autotuning settings which
will alter thingsfor the next autotuning flight</string>
</property>
</widget>
<widget class="QPushButton" name="useComputedValues">
<property name="geometry">
<rect>
<x>250</x>
<y>380</y>
<width>171</width>
<height>31</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">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;
}</string>
</property>
<property name="text">
<string>Apply Computed Values</string>
</property>
</widget>
<widget class="QLabel" name="label_21">
<property name="geometry">
<rect>
<x>40</x>
<y>410</y>
<width>104</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>Step Size</string>
</property>
</widget>
<widget class="QSlider" name="stepSizeSlider">
<property name="geometry">
<rect>
<x>149</x>
<y>410</y>
<width>266</width>
<height>20</height>
</rect>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuningSettings</string>
<string>fieldname:Amplitude</string>
<string>scale:0.01</string>
<string>haslimits:no</string>
</stringlist>
</property>
</widget>
</widget>
<resources/>
<connections/>
</ui>

16
ground/openpilotgcs/src/plugins/config/config.pro
View File

@ -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

136
ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp Normal file
View File

@ -0,0 +1,136 @@
#include "configautotunewidget.h"
#include <QDebug>
#include <QStringList>
#include <QtGui/QWidget>
#include <QtGui/QTextEdit>
#include <QtGui/QVBoxLayout>
#include <QtGui/QPushButton>
#include <QDesktopServices>
#include <QUrl>
#include <QList>
#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]));
}

60
ground/openpilotgcs/src/plugins/config/configautotunewidget.h Normal file
View File

@ -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 <QtGui/QWidget>
#include <QTimer>
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

53
ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
View File

@ -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()

4
ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
View File

@ -34,7 +34,6 @@
#include "uavobject.h"
#include <QtGui/QWidget>
#include <QTimer>
#include <QMutex>
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<double> x_accum, y_accum, z_accum;
QList<double> 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);

4
ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
View File

@ -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"));

2
ground/openpilotgcs/src/plugins/config/configgadgetwidget.h
View File

@ -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:

186
ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp
View File

@ -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<IConnection>(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<IConnection>(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<devListItem>::iterator iter = m_devList.begin(); iter != m_devList.end(); )
// Get the updated list of devices
QList <IConnection::device> 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<DevListItem>::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<IConnection::device> 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

49
ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h
View File

@ -32,6 +32,7 @@
#include <QWidget>
#include "mainwindow.h"
#include "generalsettings.h"
#include <coreplugin/iconnection.h>
#include <QtCore/QVector>
#include <QtCore/QIODevice>
#include <QtCore/QLinkedList>
@ -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<devListItem> getAvailableDevices() { return m_devList; }
QLinkedList<DevListItem> 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<Core::devListItem> devices);
void availableDevicesChanged(const QLinkedList<Core::DevListItem> 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<devListItem> m_devList;
QLinkedList<DevListItem> m_devList;
QList<IConnection*> 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 <IConnection *> connectionBackup;

8
ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp
View File

@ -174,10 +174,8 @@ QList <Core::IConnection::device> 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);

512
ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp.orig
View File

@ -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 <coreplugin/icore.h>
#include "notificationitem.h"
#include "ui_notifypluginoptionspage.h"
#include "extensionsystem/pluginmanager.h"
#include "utils/pathutils.h"
#include <QFileDialog>
#include <QtAlgorithms>
#include <QStringList>
#include <QtCore/QSettings>
#include <QTableWidget>
#include <QPalette>
#include <QBuffer>
#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<UAVObjectManager>())
, owner(qobject_cast<SoundNotifyPlugin*>(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<UAVDataObject*> > objList = objManager.getDataObjects();
foreach (QList<UAVDataObject*> 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<UAVObjectManager>();
// Fills the combo boxes for the UAVObjects
QList< QList<UAVDataObject*> > objList = objManager->getDataObjects();
foreach (QList<UAVDataObject*> 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<NotifyPluginConfiguration*>)),
owner, SLOT(updateNotificationList(QList<NotifyPluginConfiguration*>)));
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<NotificationItem*>)),
owner, SLOT(updateNotificationList(QList<NotificationItem*>)));
//connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification()));
// privListNotifications = ((qobject_cast<SoundNotifyPlugin*>(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 <Field> combo box when value is changed in the
// <Object> combo box
//////////////////////////////////////////////////////////////////////////////
void NotifyPluginOptionsPage::on_UAVObject_indexChanged(QString val) {
options_page->UAVObjectField->clear();
ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager* objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(val) );
QList<UAVObjectField*> 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<Phonon::Path> 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<UAVDataObject*>(objManager.getObject(uavDataObject));
if (obj != NULL ) {
QList<UAVObjectField*> 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());
}

2
ground/openpilotgcs/src/plugins/plugins.pro
View File

@ -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

39
ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h
View File

@ -32,11 +32,15 @@
#include <stdlib.h>
#include <stdint.h>
#include <QDebug>
#include <QMutex>
#include <QString>
#include "rawhid_global.h"
#if defined( Q_OS_MAC)
#include <IOKit/IOKitLib.h>
#include <IOKit/hid/IOHIDLib.h>
#include <CoreFoundation/CFString.h>
#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;

572
ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp
View File

@ -40,102 +40,67 @@
#include "pjrc_rawhid.h"
#include <unistd.h>
#include <IOKit/IOKitLib.h>
#include <IOKit/hid/IOHIDLib.h>
#include <CoreFoundation/CFString.h>
#include <QString>
#include <QThread>
#include <QTimer>
#include <QCoreApplication>
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

149
ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp
View File

@ -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<USBPortInfo> 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();
}

8
ground/openpilotgcs/src/plugins/rawhid/rawhid.h
View File

@ -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

5
ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp
View File

@ -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()

1
ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.h
View File

@ -31,7 +31,6 @@
#include "rawhid_global.h"
#include "rawhid.h"
#include "usbmonitor.h"
#include "usbsignalfilter.h"
#include "coreplugin/iconnection.h"
#include <extensionsystem/iplugin.h>

12
ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp
View File

@ -40,7 +40,7 @@ ControllerPage::ControllerPage(SetupWizard *wizard, QWidget *parent) :
m_connectionManager = getWizard()->getConnectionManager();
Q_ASSERT(m_connectionManager);
connect(m_connectionManager, SIGNAL(availableDevicesChanged(QLinkedList<Core::devListItem>)), this, SLOT(devicesChanged(QLinkedList<Core::devListItem>)));
connect(m_connectionManager, SIGNAL(availableDevicesChanged(QLinkedList<Core::DevListItem>)), this, SLOT(devicesChanged(QLinkedList<Core::DevListItem>)));
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<Core::devListItem> devices)
void ControllerPage::devicesChanged(QLinkedList<Core::DevListItem> devices)
{
// Get the selected item before the update if any
QString currSelectedDeviceName = ui->deviceCombo->currentIndex() != -1 ?
@ -145,10 +145,10 @@ void ControllerPage::devicesChanged(QLinkedList<Core::devListItem> 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);

2
ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h
View File

@ -58,7 +58,7 @@ private:
Core::ConnectionManager *m_connectionManager;
private slots:
void devicesChanged(QLinkedList<Core::devListItem> devices);
void devicesChanged(QLinkedList<Core::DevListItem> devices);
void connectionStatusChanged();
void connectDisconnect();
};

4
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -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 \

51
ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp
View File

@ -87,11 +87,13 @@ DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname):
m_eventloop.exec();
QList<USBPortInfo> 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:"<<numberOfPackets<<"packets"<<" Last Packet Size="<<lastPacketCount<<" "<<result << " bytes sent";
float percentage;
@ -457,7 +459,6 @@ bool DFUObject::StartDownloadT(QByteArray *fw, qint32 const & numberOfBytes, Tra
laspercentage=(int)percentage;
result = receiveData(buf,BUF_LEN);
//result = hidHandle.receive(0,buf,BUF_LEN,5000);
if(debug)
qDebug() << result << " bytes received"<<" Count="<<x<<"-"<<(int)buf[2]<<";"<<(int)buf[3]<<";"<<(int)buf[4]<<";"<<(int)buf[5]<<" Data="<<(int)buf[6]<<";"<<(int)buf[7]<<";"<<(int)buf[8]<<";"<<(int)buf[9];
if(x==numberOfPackets-1)
@ -508,7 +509,6 @@ int DFUObject::AbortOperation(void)
buf[9] = 0;
return sendData(buf, BUF_LEN);
//return hidHandle.send(0,buf, BUF_LEN, 500);
}
/**
@ -538,7 +538,6 @@ int DFUObject::JumpToApp(bool safeboot)
}
return sendData(buf, BUF_LEN);
//return hidHandle.send(0,buf, BUF_LEN, 500);
}
OP_DFU::Status DFUObject::StatusRequest()
@ -556,11 +555,9 @@ OP_DFU::Status DFUObject::StatusRequest()
buf[9] = 0;
int result = sendData(buf, BUF_LEN);
//int result = hidHandle.send(0,buf, BUF_LEN, 10000);
if(debug)
qDebug() << "StatusRequest: " << result << " bytes sent";
result = receiveData(buf,BUF_LEN);
// result = hidHandle.receive(0,buf,BUF_LEN,10000);
if(debug)
qDebug() << "StatusRequest: " << result << " bytes received";
if(buf[1]==OP_DFU::Status_Rep)
@ -590,22 +587,17 @@ bool DFUObject::findDevices()
buf[9] = 0;
int result = sendData(buf, BUF_LEN);
//int result = hidHandle.send(0,buf, BUF_LEN, 5000);
if(result<1)
{
if (result < 1)
return false;
}
result = receiveData(buf,BUF_LEN);
//result = hidHandle.receive(0,buf,BUF_LEN,5000);
if(result<1)
{
if (result < 1)
return false;
}
numberOfDevices=buf[7];
RWFlags=buf[8];
RWFlags=RWFlags<<8 | buf[9];
if(buf[1]==OP_DFU::Rep_Capabilities)
{
for(int x=0;x<numberOfDevices;++x)
@ -626,9 +618,6 @@ bool DFUObject::findDevices()
buf[9] = 0;
int result = sendData(buf, BUF_LEN);
result = receiveData(buf,BUF_LEN);
// int result = hidHandle.send(0,buf, BUF_LEN, 5000);
// result = hidHandle.receive(0,buf,BUF_LEN,5000);
//devices[x].ID=buf[9];
devices[x].ID=buf[14];
devices[x].ID=devices[x].ID<<8 | (quint8)buf[15];
devices[x].BL_Version=buf[7];
@ -684,8 +673,6 @@ bool DFUObject::EndOperation()
buf[9] = 0;
int result = sendData(buf, BUF_LEN);
// int result = hidHandle.send(0,buf, BUF_LEN, 5000);
// hidHandle.receive(0,buf,BUF_LEN,5000);
if(debug)
qDebug() << result << " bytes sent";
if(result>0)

29
ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp
View File

@ -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)

2
shared/uavobjectdefinition/flightstatus.xml
View File

@ -4,7 +4,7 @@
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
<!-- Note these enumerated values should be the same as ManualControlSettings -->
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold"/>
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

2
shared/uavobjectdefinition/hwsettings.xml
View File

@ -18,7 +18,7 @@
<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,RCTransmitter,Disabled" defaultvalue="USBTelemetry"/>
<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,Disabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,TxPID" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,TxPID,Autotune" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>
<access gcs="readwrite" flight="readwrite"/>

8
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -18,13 +18,13 @@
<field name="Arming" units="" type="enum" elements="1" options="Always Disarmed,Always Armed,Roll Left,Roll Right,Pitch Forward,Pitch Aft,Yaw Left,Yaw Right" defaultvalue="Always Disarmed"/>
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
<field name="Stabilization1Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar" defaultvalue="Attitude,Attitude,Rate"/>
<field name="Stabilization2Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar" defaultvalue="Attitude,Attitude,Rate"/>
<field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar" defaultvalue="Attitude,Attitude,Rate"/>
<field name="Stabilization1Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude" defaultvalue="Attitude,Attitude,Rate"/>
<field name="Stabilization2Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude" defaultvalue="Attitude,Attitude,Rate"/>
<field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude" defaultvalue="Attitude,Attitude,Rate"/>
<!-- Note these options values should be identical to those defined in FlightMode -->
<field name="FlightModeNumber" units="" type="uint8" elements="1" defaultvalue="3"/>
<field name="FlightModePosition" units="" type="enum" elements="6" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold" defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized1,Stabilized2" limits="%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold"/>
<field name="FlightModePosition" units="" type="enum" elements="6" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold" defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized1,Stabilized2" limits="%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<access gcs="readwrite" flight="readwrite"/>

11
shared/uavobjectdefinition/relaytuning.xml Normal file
View File

@ -0,0 +1,11 @@
<xml>
<object name="RelayTuning" singleinstance="true" settings="false">
<description>The input to the relay tuning.</description>
<field name="Period" units="ms" type="float" elementnames="Roll,Pitch,Yaw"/>
<field name="Gain" units="(deg/s)/output" type="float" elementnames="Roll,Pitch,Yaw"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

15
shared/uavobjectdefinition/relaytuningsettings.xml Normal file
View File

@ -0,0 +1,15 @@
<xml>
<object name="RelayTuningSettings" singleinstance="true" settings="true">
<description>Setting to run a relay tuning algorithm</description>
<field name="RateGain" units="" type="float" elements="1" defaultvalue="0.3333"/>
<field name="AttitudeGain" units="" type="float" elements="1" defaultvalue="0.2"/>
<field name="Amplitude" units="" type="float" elements="1" defaultvalue="0.25"/>
<field name="HysteresisThresh" units="deg/s" type="uint8" elements="1" defaultvalue="5"/>
<field name="Mode" units="" type="enum" elements="1" options="Rate,Attitude" defaultvalue="Attitude"/>
<field name="Behavior" units="" type="enum" elements="1" options="Measure,Compute,Save" defaultvalue="Compute"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

2
shared/uavobjectdefinition/stabilizationdesired.xml
View File

@ -6,7 +6,7 @@
<field name="Yaw" units="degrees" type="float" elements="1"/>
<field name="Throttle" units="%" type="float" elements="1"/>
<!-- These values should match those in ManualControlCommand.Stabilization{1,2,3}Settings -->
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar"/>
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

2
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -24,6 +24,8 @@
<field name="VbarMaxAngle" units="deg" type="uint8" elements="1" defaultvalue="10"/>
<field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.005"/>
<field name="DerivativeCutoff" units="Hz" type="uint8" elements="1" defaultvalue="20"/>
<field name="DerivativeGamma" units="" type="float" elements="1" defaultvalue="1"/>
<field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="15"/>
<field name="MaxAxisLockRate" units="deg/s" type="uint8" elements="1" defaultvalue="2"/>

6
shared/uavobjectdefinition/taskinfo.xml
View File

@ -1,9 +1,9 @@
<xml>
<object name="TaskInfo" singleinstance="true" settings="false">
<description>Task information</description>
<field name="StackRemaining" units="bytes" type="uint16" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,EventDispatcher"/>
<field name="Running" units="bool" type="enum" options="False,True" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,EventDispatcher"/>
<field name="RunningTime" units="%" type="uint8" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,EventDispatcher"/>
<field name="StackRemaining" units="bytes" type="uint16" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,Autotune,EventDispatcher"/>
<field name="Running" units="bool" type="enum" options="False,True" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,Autotune,EventDispatcher"/>
<field name="RunningTime" units="%" type="uint8" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Stabilization,AltitudeHold,Guidance,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,Autotune,EventDispatcher"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="periodic" period="10000"/>