diff --git a/flight/OpenPilot/Modules/ManualControl/manualcontrol.c b/flight/OpenPilot/Modules/ManualControl/manualcontrol.c index 2b0f50ecf..101ae8197 100644 --- a/flight/OpenPilot/Modules/ManualControl/manualcontrol.c +++ b/flight/OpenPilot/Modules/ManualControl/manualcontrol.c @@ -152,6 +152,12 @@ static void manualControlTask(void* parameters) settings.ChannelMin[settings.Accessory2], settings.ChannelNeutral[settings.Accessory2] ); else cmd.Accessory2 = 0; + + if(settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) + cmd.Accessory3 = scaleChannel( cmd.Channel[settings.Accessory3], settings.ChannelMax[settings.Accessory3], + settings.ChannelMin[settings.Accessory3], settings.ChannelNeutral[settings.Accessory3] ); + else + cmd.Accessory3 = 0; // Update flight mode @@ -223,13 +229,23 @@ static void manualControlTask(void* parameters) AttitudeDesiredSet(&attitude); } - if( 1 ) { //TODO: Make what happens here depend on GCS + if( cmd.Accessory3 < -.5 ) { //TODO: Make what happens here depend on GCS AttitudeSettingsData attitudeSettings; AttitudeSettingsGet(&attitudeSettings); - // Hard coding a maximum bias of 30 for now... maybe mistake - attitudeSettings.PitchBias = cmd.Accessory1 * 30; - attitudeSettings.RollBias = cmd.Accessory2 * 30; + // Hard coding a maximum bias of 15 for now... maybe mistake + attitudeSettings.PitchBias = cmd.Accessory1 * 15; + attitudeSettings.RollBias = cmd.Accessory2 * 15; AttitudeSettingsSet(&attitudeSettings); + } else if (cmd.Accessory3 > .9) { + // REALLY don't want to end up here accidentally. I've also saved by meta for Stabilization setting to be + // flight read only by default + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); + if(cmd.Accessory1 > 0) + stabSettings.PitchKp = cmd.Accessory1 * 0.05; + if(cmd.Accessory2 > 0) + stabSettings.RollKp = cmd.Accessory2 * 0.05; + StabilizationSettingsSet(&stabSettings); } } } diff --git a/flight/OpenPilot/UAVObjects/inc/manualcontrolcommand.h b/flight/OpenPilot/UAVObjects/inc/manualcontrolcommand.h index 0bb457ee8..6a02aca55 100644 --- a/flight/OpenPilot/UAVObjects/inc/manualcontrolcommand.h +++ b/flight/OpenPilot/UAVObjects/inc/manualcontrolcommand.h @@ -41,7 +41,7 @@ #define MANUALCONTROLCOMMAND_H // Object constants -#define MANUALCONTROLCOMMAND_OBJID 540381354U +#define MANUALCONTROLCOMMAND_OBJID 1289277596U #define MANUALCONTROLCOMMAND_NAME "ManualControlCommand" #define MANUALCONTROLCOMMAND_METANAME "ManualControlCommandMeta" #define MANUALCONTROLCOMMAND_ISSINGLEINST 1 @@ -79,6 +79,7 @@ typedef struct { uint8_t FlightMode; float Accessory1; float Accessory2; + float Accessory3; int16_t Channel[8]; } __attribute__((packed)) ManualControlCommandData; @@ -96,6 +97,7 @@ typedef enum { MANUALCONTROLCOMMAND_CONNECTED_FALSE=0, MANUALCONTROLCOMMAND_CONN typedef enum { MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL=0, MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED=1, MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO=2 } ManualControlCommandFlightModeOptions; // Field Accessory1 information // Field Accessory2 information +// Field Accessory3 information // Field Channel information /* Number of elements for field Channel */ #define MANUALCONTROLCOMMAND_CHANNEL_NUMELEM 8 diff --git a/flight/OpenPilot/UAVObjects/inc/manualcontrolsettings.h b/flight/OpenPilot/UAVObjects/inc/manualcontrolsettings.h index dd612f449..4466b4ff7 100644 --- a/flight/OpenPilot/UAVObjects/inc/manualcontrolsettings.h +++ b/flight/OpenPilot/UAVObjects/inc/manualcontrolsettings.h @@ -41,7 +41,7 @@ #define MANUALCONTROLSETTINGS_H // Object constants -#define MANUALCONTROLSETTINGS_OBJID 2185031164U +#define MANUALCONTROLSETTINGS_OBJID 1845767664U #define MANUALCONTROLSETTINGS_NAME "ManualControlSettings" #define MANUALCONTROLSETTINGS_METANAME "ManualControlSettingsMeta" #define MANUALCONTROLSETTINGS_ISSINGLEINST 1 @@ -79,6 +79,7 @@ typedef struct { uint8_t FlightMode; uint8_t Accessory1; uint8_t Accessory2; + uint8_t Accessory3; int16_t ChannelMax[8]; int16_t ChannelNeutral[8]; int16_t ChannelMin[8]; @@ -110,6 +111,9 @@ typedef enum { MANUALCONTROLSETTINGS_ACCESSORY1_CHANNEL0=0, MANUALCONTROLSETTING // Field Accessory2 information /* Enumeration options for field Accessory2 */ typedef enum { MANUALCONTROLSETTINGS_ACCESSORY2_CHANNEL0=0, MANUALCONTROLSETTINGS_ACCESSORY2_CHANNEL1=1, MANUALCONTROLSETTINGS_ACCESSORY2_CHANNEL2=2, MANUALCONTROLSETTINGS_ACCESSORY2_CHANNEL3=3, MANUALCONTROLSETTINGS_ACCESSORY2_CHANNEL4=4, MANUALCONTROLSETTINGS_ACCESSORY2_CHANNEL5=5, MANUALCONTROLSETTINGS_ACCESSORY2_CHANNEL6=6, MANUALCONTROLSETTINGS_ACCESSORY2_CHANNEL7=7, MANUALCONTROLSETTINGS_ACCESSORY2_NONE=8 } ManualControlSettingsAccessory2Options; +// Field Accessory3 information +/* Enumeration options for field Accessory3 */ +typedef enum { MANUALCONTROLSETTINGS_ACCESSORY3_CHANNEL0=0, MANUALCONTROLSETTINGS_ACCESSORY3_CHANNEL1=1, MANUALCONTROLSETTINGS_ACCESSORY3_CHANNEL2=2, MANUALCONTROLSETTINGS_ACCESSORY3_CHANNEL3=3, MANUALCONTROLSETTINGS_ACCESSORY3_CHANNEL4=4, MANUALCONTROLSETTINGS_ACCESSORY3_CHANNEL5=5, MANUALCONTROLSETTINGS_ACCESSORY3_CHANNEL6=6, MANUALCONTROLSETTINGS_ACCESSORY3_CHANNEL7=7, MANUALCONTROLSETTINGS_ACCESSORY3_NONE=8 } ManualControlSettingsAccessory3Options; // Field ChannelMax information /* Number of elements for field ChannelMax */ #define MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM 8 diff --git a/flight/OpenPilot/UAVObjects/manualcontrolsettings.c b/flight/OpenPilot/UAVObjects/manualcontrolsettings.c index ed72b5402..5bce854f2 100644 --- a/flight/OpenPilot/UAVObjects/manualcontrolsettings.c +++ b/flight/OpenPilot/UAVObjects/manualcontrolsettings.c @@ -88,6 +88,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId) data.FlightMode = 4; data.Accessory1 = 8; data.Accessory2 = 8; + data.Accessory3 = 8; data.ChannelMax[0] = 2000; data.ChannelMax[1] = 2000; data.ChannelMax[2] = 2000; diff --git a/ground/src/plugins/uavobjects/manualcontrolcommand.cpp b/ground/src/plugins/uavobjects/manualcontrolcommand.cpp index 2920170dc..f572feb81 100644 --- a/ground/src/plugins/uavobjects/manualcontrolcommand.cpp +++ b/ground/src/plugins/uavobjects/manualcontrolcommand.cpp @@ -73,6 +73,9 @@ ManualControlCommand::ManualControlCommand(): UAVDataObject(OBJID, ISSINGLEINST, QStringList Accessory2ElemNames; Accessory2ElemNames.append("0"); fields.append( new UAVObjectField(QString("Accessory2"), QString("%"), UAVObjectField::FLOAT32, Accessory2ElemNames, QStringList()) ); + QStringList Accessory3ElemNames; + Accessory3ElemNames.append("0"); + fields.append( new UAVObjectField(QString("Accessory3"), QString("%"), UAVObjectField::FLOAT32, Accessory3ElemNames, QStringList()) ); QStringList ChannelElemNames; ChannelElemNames.append("0"); ChannelElemNames.append("1"); diff --git a/ground/src/plugins/uavobjects/manualcontrolcommand.h b/ground/src/plugins/uavobjects/manualcontrolcommand.h index 1f11e4e9d..7b209827a 100644 --- a/ground/src/plugins/uavobjects/manualcontrolcommand.h +++ b/ground/src/plugins/uavobjects/manualcontrolcommand.h @@ -51,6 +51,7 @@ public: quint8 FlightMode; float Accessory1; float Accessory2; + float Accessory3; qint16 Channel[8]; } __attribute__((packed)) DataFields; @@ -68,13 +69,14 @@ public: typedef enum { FLIGHTMODE_MANUAL=0, FLIGHTMODE_STABILIZED=1, FLIGHTMODE_AUTO=2 } FlightModeOptions; // Field Accessory1 information // Field Accessory2 information + // Field Accessory3 information // Field Channel information /* Number of elements for field Channel */ static const quint32 CHANNEL_NUMELEM = 8; // Constants - static const quint32 OBJID = 540381354U; + static const quint32 OBJID = 1289277596U; static const QString NAME; static const bool ISSINGLEINST = 1; static const bool ISSETTINGS = 0; diff --git a/ground/src/plugins/uavobjects/manualcontrolcommand.py b/ground/src/plugins/uavobjects/manualcontrolcommand.py index 7253f1902..e8600d719 100644 --- a/ground/src/plugins/uavobjects/manualcontrolcommand.py +++ b/ground/src/plugins/uavobjects/manualcontrolcommand.py @@ -122,6 +122,16 @@ _fields = [ \ { } ), + uavobject.UAVObjectField( + 'Accessory3', + 'f', + 1, + [ + '0', + ], + { + } + ), uavobject.UAVObjectField( 'Channel', 'h', @@ -144,7 +154,7 @@ _fields = [ \ class ManualControlCommand(uavobject.UAVObject): ## Object constants - OBJID = 540381354 + OBJID = 1289277596 NAME = "ManualControlCommand" METANAME = "ManualControlCommandMeta" ISSINGLEINST = 1 diff --git a/ground/src/plugins/uavobjects/manualcontrolsettings.cpp b/ground/src/plugins/uavobjects/manualcontrolsettings.cpp index 4aaea3df0..61a9152d8 100644 --- a/ground/src/plugins/uavobjects/manualcontrolsettings.cpp +++ b/ground/src/plugins/uavobjects/manualcontrolsettings.cpp @@ -140,6 +140,19 @@ ManualControlSettings::ManualControlSettings(): UAVDataObject(OBJID, ISSINGLEINS Accessory2EnumOptions.append("Channel7"); Accessory2EnumOptions.append("None"); fields.append( new UAVObjectField(QString("Accessory2"), QString("channel"), UAVObjectField::ENUM, Accessory2ElemNames, Accessory2EnumOptions) ); + QStringList Accessory3ElemNames; + Accessory3ElemNames.append("0"); + QStringList Accessory3EnumOptions; + Accessory3EnumOptions.append("Channel0"); + Accessory3EnumOptions.append("Channel1"); + Accessory3EnumOptions.append("Channel2"); + Accessory3EnumOptions.append("Channel3"); + Accessory3EnumOptions.append("Channel4"); + Accessory3EnumOptions.append("Channel5"); + Accessory3EnumOptions.append("Channel6"); + Accessory3EnumOptions.append("Channel7"); + Accessory3EnumOptions.append("None"); + fields.append( new UAVObjectField(QString("Accessory3"), QString("channel"), UAVObjectField::ENUM, Accessory3ElemNames, Accessory3EnumOptions) ); QStringList ChannelMaxElemNames; ChannelMaxElemNames.append("0"); ChannelMaxElemNames.append("1"); @@ -211,6 +224,7 @@ void ManualControlSettings::setDefaultFieldValues() data.FlightMode = 4; data.Accessory1 = 8; data.Accessory2 = 8; + data.Accessory3 = 8; data.ChannelMax[0] = 2000; data.ChannelMax[1] = 2000; data.ChannelMax[2] = 2000; diff --git a/ground/src/plugins/uavobjects/manualcontrolsettings.h b/ground/src/plugins/uavobjects/manualcontrolsettings.h index 5c616878b..1fa158713 100644 --- a/ground/src/plugins/uavobjects/manualcontrolsettings.h +++ b/ground/src/plugins/uavobjects/manualcontrolsettings.h @@ -51,6 +51,7 @@ public: quint8 FlightMode; quint8 Accessory1; quint8 Accessory2; + quint8 Accessory3; qint16 ChannelMax[8]; qint16 ChannelNeutral[8]; qint16 ChannelMin[8]; @@ -82,6 +83,9 @@ public: // Field Accessory2 information /* Enumeration options for field Accessory2 */ typedef enum { ACCESSORY2_CHANNEL0=0, ACCESSORY2_CHANNEL1=1, ACCESSORY2_CHANNEL2=2, ACCESSORY2_CHANNEL3=3, ACCESSORY2_CHANNEL4=4, ACCESSORY2_CHANNEL5=5, ACCESSORY2_CHANNEL6=6, ACCESSORY2_CHANNEL7=7, ACCESSORY2_NONE=8 } Accessory2Options; + // Field Accessory3 information + /* Enumeration options for field Accessory3 */ + typedef enum { ACCESSORY3_CHANNEL0=0, ACCESSORY3_CHANNEL1=1, ACCESSORY3_CHANNEL2=2, ACCESSORY3_CHANNEL3=3, ACCESSORY3_CHANNEL4=4, ACCESSORY3_CHANNEL5=5, ACCESSORY3_CHANNEL6=6, ACCESSORY3_CHANNEL7=7, ACCESSORY3_NONE=8 } Accessory3Options; // Field ChannelMax information /* Number of elements for field ChannelMax */ static const quint32 CHANNELMAX_NUMELEM = 8; @@ -94,7 +98,7 @@ public: // Constants - static const quint32 OBJID = 2185031164U; + static const quint32 OBJID = 1845767664U; static const QString NAME; static const bool ISSINGLEINST = 1; static const bool ISSETTINGS = 1; diff --git a/ground/src/plugins/uavobjects/manualcontrolsettings.py b/ground/src/plugins/uavobjects/manualcontrolsettings.py index fdd87586a..83c6b537e 100644 --- a/ground/src/plugins/uavobjects/manualcontrolsettings.py +++ b/ground/src/plugins/uavobjects/manualcontrolsettings.py @@ -183,6 +183,25 @@ _fields = [ \ '8' : 'None', } ), + uavobject.UAVObjectField( + 'Accessory3', + 'b', + 1, + [ + '0', + ], + { + '0' : 'Channel0', + '1' : 'Channel1', + '2' : 'Channel2', + '3' : 'Channel3', + '4' : 'Channel4', + '5' : 'Channel5', + '6' : 'Channel6', + '7' : 'Channel7', + '8' : 'None', + } + ), uavobject.UAVObjectField( 'ChannelMax', 'h', @@ -239,7 +258,7 @@ _fields = [ \ class ManualControlSettings(uavobject.UAVObject): ## Object constants - OBJID = 2185031164 + OBJID = 1845767664 NAME = "ManualControlSettings" METANAME = "ManualControlSettingsMeta" ISSINGLEINST = 1 diff --git a/ground/src/shared/uavobjectdefinition/manualcontrolcommand.xml b/ground/src/shared/uavobjectdefinition/manualcontrolcommand.xml index b6edb46ab..fd7a66af3 100644 --- a/ground/src/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/ground/src/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -9,6 +9,7 @@ + diff --git a/ground/src/shared/uavobjectdefinition/manualcontrolsettings.xml b/ground/src/shared/uavobjectdefinition/manualcontrolsettings.xml index fceb843d8..c7cacfb3a 100644 --- a/ground/src/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/ground/src/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -9,6 +9,7 @@ +