rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-05 21:52:10 +01:00
Code Issues Releases Activity

Flight: Got a much nicer arming/disarming system going. Made it a separate flag in the end to allow testing the stabilization code with motors not running. Also changes maxRoll/maxPitch/maxYaw to uint8 to keep object size smaller - no reason for float precision for this.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1660 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-16 15:41:11 +00:00 committed by peabody124
parent a6e71177e4
commit 3b015b41c5
17 changed files with 173 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
flight/OpenPilot/Modules/Actuator/actuator.c
View File

@ -38,6 +38,7 @@
#include "systemsettings.h" #include "systemsettings.h"
#include "actuatordesired.h" #include "actuatordesired.h"
#include "actuatorcommand.h" #include "actuatorcommand.h"
#include "manualcontrolcommand.h"
// Private constants // Private constants
#define MAX_QUEUE_SIZE 2 #define MAX_QUEUE_SIZE 2
@ -63,7 +64,6 @@ static void setFailsafe();
static float bound(float val, float min, float max); static float bound(float val, float min, float max);
static uint8_t vtolEnabled = 0;
/** /**
* @brief Module initialization * @brief Module initialization
* @return 0 * @return 0
@ -98,8 +98,6 @@ static void actuatorTask(void* parameters)
ActuatorSettingsGet(&settings); ActuatorSettingsGet(&settings);
PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]); PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]);
vtolEnabled = 0; // a flag that must be set by moving throttle up and down again
// Go to the neutral (failsafe) values until an ActuatorDesired update is received // Go to the neutral (failsafe) values until an ActuatorDesired update is received
setFailsafe(); setFailsafe();
@ -196,6 +194,7 @@ static void actuatorTask(void* parameters)
*/ */
static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd) static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd)
{ {
// Check settings // Check settings
if ( settings->FixedWingPitch1 == ACTUATORSETTINGS_FIXEDWINGPITCH1_NONE || if ( settings->FixedWingPitch1 == ACTUATORSETTINGS_FIXEDWINGPITCH1_NONE ||
settings->FixedWingRoll1 == ACTUATORSETTINGS_FIXEDWINGROLL1_NONE || settings->FixedWingRoll1 == ACTUATORSETTINGS_FIXEDWINGROLL1_NONE ||
@ -204,6 +203,9 @@ static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const Actuat
return -1; return -1;
} }
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
// Set pitch servo command // Set pitch servo command
cmd->Channel[ settings->FixedWingPitch1 ] = scaleChannel(desired->Pitch, settings->ChannelMax[ settings->FixedWingPitch1 ], cmd->Channel[ settings->FixedWingPitch1 ] = scaleChannel(desired->Pitch, settings->ChannelMax[ settings->FixedWingPitch1 ],
settings->ChannelMin[ settings->FixedWingPitch1 ], settings->ChannelMin[ settings->FixedWingPitch1 ],
@ -235,9 +237,12 @@ static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const Actuat
} }
// Set throttle servo command // Set throttle servo command
cmd->Channel[ settings->FixedWingThrottle ] = scaleChannel(desired->Throttle, settings->ChannelMax[ settings->FixedWingThrottle ], if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE)
cmd->Channel[ settings->FixedWingThrottle ] = scaleChannel(desired->Throttle, settings->ChannelMax[ settings->FixedWingThrottle ],
settings->ChannelMin[ settings->FixedWingThrottle ], settings->ChannelMin[ settings->FixedWingThrottle ],
settings->ChannelNeutral[ settings->FixedWingThrottle ]); settings->ChannelNeutral[ settings->FixedWingThrottle ]);
else
cmd->Channel[ settings->FixedWingThrottle ] = -1;
// Done // Done
return 0; return 0;
@ -257,20 +262,26 @@ static int32_t mixerFixedWingElevon(const ActuatorSettingsData* settings, const
return -1; return -1;
} }
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
// Set first elevon servo command // Set first elevon servo command
cmd->Channel[ settings->FixedWingRoll1 ] = scaleChannel(desired->Pitch + desired->Roll, settings->ChannelMax[ settings->FixedWingRoll1 ], cmd->Channel[ settings->FixedWingRoll1 ] = scaleChannel(desired->Pitch + desired->Roll, settings->ChannelMax[ settings->FixedWingRoll1 ],
settings->ChannelMin[ settings->FixedWingRoll1 ], settings->ChannelMin[ settings->FixedWingRoll1 ],
settings->ChannelNeutral[ settings->FixedWingRoll1 ]); settings->ChannelNeutral[ settings->FixedWingRoll1 ]);
// Set second elevon servo command // Set second elevon servo command
cmd->Channel[ settings->FixedWingRoll2 ] = scaleChannel(desired->Pitch - desired->Roll, settings->ChannelMax[ settings->FixedWingRoll2 ], cmd->Channel[ settings->FixedWingRoll2 ] = scaleChannel(desired->Pitch - desired->Roll, settings->ChannelMax[ settings->FixedWingRoll2 ],
settings->ChannelMin[ settings->FixedWingRoll2 ], settings->ChannelMin[ settings->FixedWingRoll2 ],
settings->ChannelNeutral[ settings->FixedWingRoll2 ]); settings->ChannelNeutral[ settings->FixedWingRoll2 ]);
// Set throttle servo command // Set throttle servo command
cmd->Channel[ settings->FixedWingThrottle ] = scaleChannel(desired->Throttle, settings->ChannelMax[ settings->FixedWingThrottle ], if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE)
settings->ChannelMin[ settings->FixedWingThrottle ], cmd->Channel[ settings->FixedWingThrottle ] = scaleChannel(desired->Throttle, settings->ChannelMax[ settings->FixedWingThrottle ],
settings->ChannelNeutral[ settings->FixedWingThrottle ]); settings->ChannelMin[ settings->FixedWingThrottle ],
settings->ChannelNeutral[ settings->FixedWingThrottle ]);
else
cmd->Channel[ settings->FixedWingThrottle ] = -1;
// Done // Done
return 0; return 0;
@ -294,19 +305,17 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
VTOLSettingsGet(&vtolSettings); VTOLSettingsGet(&vtolSettings);
VTOLStatusGet(&vtolStatus); VTOLStatusGet(&vtolStatus);
// Simple logic to require throttle past .5 and then brought back before enabling motors. ManualControlCommandData manualControl;
// This possibly should be used for other mixers. ManualControlCommandGet(&manualControl);
if ((vtolEnabled == 0) && (desired->Throttle > .5))
vtolEnabled = 1;
if ((vtolEnabled == 1) && (desired->Throttle <= 0))
vtolEnabled = 2;
const int vtolMin = -1; const int vtolMin = -1;
int vtolMax = 1; int vtolMax = -1;
if(desired->Throttle < 0 || vtolEnabled < 2)
vtolMax = -1; if((manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE) &&
else (desired->Throttle >= 0))
vtolMax = 1; vtolMax = 1;
else
vtolMax = -1;
if(((settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) + if(((settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) +
(settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORS_NONE) + (settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORS_NONE) +
@ -320,51 +329,51 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
} }
if(settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) { if(settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) {
vtolStatus.MotorN = bound(desired->Throttle * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_THROTTLE] + vtolStatus.MotorN = desired->Throttle * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_THROTTLE] +
desired->Pitch * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_PITCH] + desired->Pitch * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_PITCH] +
desired->Roll * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_ROLL] + desired->Roll * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_ROLL] +
desired->Yaw * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_YAW],vtolMin,vtolMax); desired->Yaw * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_YAW];
cmd->Channel[settings->VTOLMotorN] = scaleChannel(vtolStatus.MotorN, cmd->Channel[settings->VTOLMotorN] = scaleChannel(bound(vtolStatus.MotorN, vtolMin, vtolMax),
settings->ChannelMax[settings->VTOLMotorN], settings->ChannelMax[settings->VTOLMotorN],
settings->ChannelMin[settings->VTOLMotorN], settings->ChannelMin[settings->VTOLMotorN],
settings->ChannelNeutral[settings->VTOLMotorN]); settings->ChannelNeutral[settings->VTOLMotorN]);
} }
if(settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORNE_NONE) { if(settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORNE_NONE) {
vtolStatus.MotorNE = bound(desired->Throttle * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_THROTTLE] + vtolStatus.MotorNE = desired->Throttle * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_THROTTLE] +
desired->Pitch * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_PITCH] + desired->Pitch * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_PITCH] +
desired->Roll * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_ROLL] + desired->Roll * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_ROLL] +
desired->Yaw * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_YAW],vtolMin,vtolMax); desired->Yaw * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_YAW];
cmd->Channel[settings->VTOLMotorNE] = scaleChannel(vtolStatus.MotorNE, cmd->Channel[settings->VTOLMotorNE] = scaleChannel(bound(vtolStatus.MotorNE, vtolMin, vtolMax),
settings->ChannelMax[settings->VTOLMotorNE], settings->ChannelMax[settings->VTOLMotorNE],
settings->ChannelMin[settings->VTOLMotorNE], settings->ChannelMin[settings->VTOLMotorNE],
settings->ChannelNeutral[settings->VTOLMotorNE]); settings->ChannelNeutral[settings->VTOLMotorNE]);
} }
if(settings->VTOLMotorE != ACTUATORSETTINGS_VTOLMOTORE_NONE) { if(settings->VTOLMotorE != ACTUATORSETTINGS_VTOLMOTORE_NONE) {
vtolStatus.MotorE = bound(desired->Throttle * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_THROTTLE] + vtolStatus.MotorE = desired->Throttle * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_THROTTLE] +
desired->Pitch * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_PITCH] + desired->Pitch * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_PITCH] +
desired->Roll * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_ROLL] + desired->Roll * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_ROLL] +
desired->Yaw * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_YAW],vtolMin,vtolMax); desired->Yaw * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_YAW];
cmd->Channel[settings->VTOLMotorE] = scaleChannel(vtolStatus.MotorE, cmd->Channel[settings->VTOLMotorE] = scaleChannel(bound(vtolStatus.MotorE, vtolMin, vtolMax),
settings->ChannelMax[settings->VTOLMotorE], settings->ChannelMax[settings->VTOLMotorE],
settings->ChannelMin[settings->VTOLMotorE], settings->ChannelMin[settings->VTOLMotorE],
settings->ChannelNeutral[settings->VTOLMotorE]); settings->ChannelNeutral[settings->VTOLMotorE]);
} }
if(settings->VTOLMotorSE != ACTUATORSETTINGS_VTOLMOTORSE_NONE) { if(settings->VTOLMotorSE != ACTUATORSETTINGS_VTOLMOTORSE_NONE) {
vtolStatus.MotorSE = bound(desired->Throttle * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_THROTTLE] + vtolStatus.MotorSE = desired->Throttle * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_THROTTLE] +
desired->Pitch * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_PITCH] + desired->Pitch * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_PITCH] +
desired->Roll * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_ROLL] + desired->Roll * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_ROLL] +
desired->Yaw * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_YAW],vtolMin,vtolMax); desired->Yaw * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_YAW];
cmd->Channel[settings->VTOLMotorSE] = scaleChannel(vtolStatus.MotorSE, cmd->Channel[settings->VTOLMotorSE] = scaleChannel(bound(vtolStatus.MotorSE, vtolMin, vtolMax),
settings->ChannelMax[settings->VTOLMotorSE], settings->ChannelMax[settings->VTOLMotorSE],
settings->ChannelMin[settings->VTOLMotorSE], settings->ChannelMin[settings->VTOLMotorSE],
settings->ChannelNeutral[settings->VTOLMotorSE]); settings->ChannelNeutral[settings->VTOLMotorSE]);
} }
if(settings->VTOLMotorS != ACTUATORSETTINGS_VTOLMOTORS_NONE) { if(settings->VTOLMotorS != ACTUATORSETTINGS_VTOLMOTORS_NONE) {
vtolStatus.MotorS = bound(desired->Throttle * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_THROTTLE] + vtolStatus.MotorS = desired->Throttle * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_THROTTLE] +
desired->Pitch * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_PITCH] + desired->Pitch * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_PITCH] +
desired->Roll * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_ROLL] + desired->Roll * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_ROLL] +
desired->Yaw * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_YAW],vtolMin,vtolMax); desired->Yaw * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_YAW];
cmd->Channel[settings->VTOLMotorS] = scaleChannel(vtolStatus.MotorS, cmd->Channel[settings->VTOLMotorS] = scaleChannel(bound(vtolStatus.MotorS, vtolMin, vtolMax),
settings->ChannelMax[settings->VTOLMotorS], settings->ChannelMax[settings->VTOLMotorS],
settings->ChannelMin[settings->VTOLMotorS], settings->ChannelMin[settings->VTOLMotorS],
settings->ChannelNeutral[settings->VTOLMotorS]); settings->ChannelNeutral[settings->VTOLMotorS]);
@ -380,21 +389,21 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
settings->ChannelNeutral[settings->VTOLMotorSW]); settings->ChannelNeutral[settings->VTOLMotorSW]);
} }
if(settings->VTOLMotorW != ACTUATORSETTINGS_VTOLMOTORW_NONE) { if(settings->VTOLMotorW != ACTUATORSETTINGS_VTOLMOTORW_NONE) {
vtolStatus.MotorW = bound(desired->Throttle * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_THROTTLE] + vtolStatus.MotorW = desired->Throttle * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_THROTTLE] +
desired->Pitch * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_PITCH] + desired->Pitch * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_PITCH] +
desired->Roll * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_ROLL] + desired->Roll * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_ROLL] +
desired->Yaw * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_YAW],vtolMin,vtolMax); desired->Yaw * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_YAW];
cmd->Channel[settings->VTOLMotorW] = scaleChannel(vtolStatus.MotorW, cmd->Channel[settings->VTOLMotorW] = scaleChannel(bound(vtolStatus.MotorW, vtolMin, vtolMax),
settings->ChannelMax[settings->VTOLMotorW], settings->ChannelMax[settings->VTOLMotorW],
settings->ChannelMin[settings->VTOLMotorW], settings->ChannelMin[settings->VTOLMotorW],
settings->ChannelNeutral[settings->VTOLMotorW]); settings->ChannelNeutral[settings->VTOLMotorW]);
} }
if(settings->VTOLMotorNW != ACTUATORSETTINGS_VTOLMOTORNW_NONE) { if(settings->VTOLMotorNW != ACTUATORSETTINGS_VTOLMOTORNW_NONE) {
vtolStatus.MotorNW = bound(desired->Throttle * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_THROTTLE] + vtolStatus.MotorNW = desired->Throttle * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_THROTTLE] +
desired->Pitch * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_PITCH] + desired->Pitch * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_PITCH] +
desired->Roll * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_ROLL] + desired->Roll * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_ROLL] +
desired->Yaw * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_YAW],vtolMin,vtolMax); desired->Yaw * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_YAW];
cmd->Channel[settings->VTOLMotorNW] = scaleChannel(vtolStatus.MotorNW, cmd->Channel[settings->VTOLMotorNW] = scaleChannel(bound(vtolStatus.MotorNW, vtolMin, vtolMax),
settings->ChannelMax[settings->VTOLMotorNW], settings->ChannelMax[settings->VTOLMotorNW],
settings->ChannelMin[settings->VTOLMotorNW], settings->ChannelMin[settings->VTOLMotorNW],
settings->ChannelNeutral[settings->VTOLMotorNW]); settings->ChannelNeutral[settings->VTOLMotorNW]);

30
flight/OpenPilot/Modules/ManualControl/manualcontrol.c
View File

@ -81,12 +81,18 @@ static void manualControlTask(void* parameters)
ActuatorDesiredData actuator; ActuatorDesiredData actuator;
AttitudeDesiredData attitude; AttitudeDesiredData attitude;
portTickType lastSysTime; portTickType lastSysTime;
portTickType armedDisarmStart = 0;
float flightMode; float flightMode;
uint8_t disconnected_count = 0; uint8_t disconnected_count = 0;
uint8_t connected_count = 0; uint8_t connected_count = 0;
enum {CONNECTED, DISCONNECTED} connection_state = DISCONNECTED; enum {CONNECTED, DISCONNECTED} connection_state = DISCONNECTED;
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
ManualControlCommandSet(&cmd);
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
while (1) while (1)
@ -229,6 +235,22 @@ static void manualControlTask(void* parameters)
ManualControlCommandSet(&cmd); ManualControlCommandSet(&cmd);
} }
/* Look for arm or disarm signal */
if ((cmd.Throttle <= 0.05) && (cmd.Roll <= -0.95)) {
if((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart)) // store when started, deal with rollover
armedDisarmStart = lastSysTime;
else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS))
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
}else if ((cmd.Throttle <= 0.05) && (cmd.Roll >= 0.95)) {
if((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart)) // store when started, deal with rollover
armedDisarmStart = lastSysTime;
else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS))
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
} else {
armedDisarmStart = 0;
}
// Depending on the mode update the Stabilization or Actuator objects // Depending on the mode update the Stabilization or Actuator objects
if ( cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL ) if ( cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
{ {
@ -255,11 +277,11 @@ static void manualControlTask(void* parameters)
{ {
attitude.Yaw = (cmd.Yaw*180.0); attitude.Yaw = (cmd.Yaw*180.0);
} }
if(cmd.Throttle < 0)
attitude.Throttle = -1;
else
attitude.Throttle = cmd.Throttle*stabSettings.ThrottleMax;
} }
if(cmd.Throttle < 0)
attitude.Throttle = -1;
else
attitude.Throttle = cmd.Throttle*stabSettings.ThrottleMax;
AttitudeDesiredSet(&attitude); AttitudeDesiredSet(&attitude);
} }

2
flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c
View File

@ -152,7 +152,7 @@ static void stabilizationTask(void* parameters)
} }
// Setup throttle // Setup throttle
actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax); actuatorDesired.Throttle = bound(attitudeDesired.Throttle, stabSettings.ThrottleMin, stabSettings.ThrottleMax);
// Write actuator desired (if not in manual mode) // Write actuator desired (if not in manual mode)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL ) if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )

6
flight/OpenPilot/UAVObjects/inc/manualcontrolcommand.h
View File

@ -41,7 +41,7 @@
#define MANUALCONTROLCOMMAND_H #define MANUALCONTROLCOMMAND_H
// Object constants // Object constants
#define MANUALCONTROLCOMMAND_OBJID 1289277596U #define MANUALCONTROLCOMMAND_OBJID 9594768U
#define MANUALCONTROLCOMMAND_NAME "ManualControlCommand" #define MANUALCONTROLCOMMAND_NAME "ManualControlCommand"
#define MANUALCONTROLCOMMAND_METANAME "ManualControlCommandMeta" #define MANUALCONTROLCOMMAND_METANAME "ManualControlCommandMeta"
#define MANUALCONTROLCOMMAND_ISSINGLEINST 1 #define MANUALCONTROLCOMMAND_ISSINGLEINST 1
@ -72,6 +72,7 @@
// Object data // Object data
typedef struct { typedef struct {
uint8_t Connected; uint8_t Connected;
uint8_t Armed;
float Roll; float Roll;
float Pitch; float Pitch;
float Yaw; float Yaw;
@ -88,6 +89,9 @@ typedef struct {
// Field Connected information // Field Connected information
/* Enumeration options for field Connected */ /* Enumeration options for field Connected */
typedef enum { MANUALCONTROLCOMMAND_CONNECTED_FALSE=0, MANUALCONTROLCOMMAND_CONNECTED_TRUE=1 } ManualControlCommandConnectedOptions; typedef enum { MANUALCONTROLCOMMAND_CONNECTED_FALSE=0, MANUALCONTROLCOMMAND_CONNECTED_TRUE=1 } ManualControlCommandConnectedOptions;
// Field Armed information
/* Enumeration options for field Armed */
typedef enum { MANUALCONTROLCOMMAND_ARMED_FALSE=0, MANUALCONTROLCOMMAND_ARMED_TRUE=1 } ManualControlCommandArmedOptions;
// Field Roll information // Field Roll information
// Field Pitch information // Field Pitch information
// Field Yaw information // Field Yaw information

12
flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h
View File

@ -41,7 +41,7 @@
#define STABILIZATIONSETTINGS_H #define STABILIZATIONSETTINGS_H
// Object constants // Object constants
#define STABILIZATIONSETTINGS_OBJID 1244337598U #define STABILIZATIONSETTINGS_OBJID 117768092U
#define STABILIZATIONSETTINGS_NAME "StabilizationSettings" #define STABILIZATIONSETTINGS_NAME "StabilizationSettings"
#define STABILIZATIONSETTINGS_METANAME "StabilizationSettingsMeta" #define STABILIZATIONSETTINGS_METANAME "StabilizationSettingsMeta"
#define STABILIZATIONSETTINGS_ISSINGLEINST 1 #define STABILIZATIONSETTINGS_ISSINGLEINST 1
@ -71,12 +71,13 @@
// Object data // Object data
typedef struct { typedef struct {
uint16_t UpdatePeriod; uint8_t UpdatePeriod;
float RollMax; uint8_t RollMax;
float PitchMax; uint8_t PitchMax;
float YawMax; uint8_t YawMax;
uint8_t YawMode; uint8_t YawMode;
float ThrottleMax; float ThrottleMax;
float ThrottleMin;
float RollIntegralLimit; float RollIntegralLimit;
float PitchIntegralLimit; float PitchIntegralLimit;
float YawIntegralLimit; float YawIntegralLimit;
@ -101,6 +102,7 @@ typedef struct {
/* Enumeration options for field YawMode */ /* Enumeration options for field YawMode */
typedef enum { STABILIZATIONSETTINGS_YAWMODE_RATE=0, STABILIZATIONSETTINGS_YAWMODE_HEADING=1 } StabilizationSettingsYawModeOptions; typedef enum { STABILIZATIONSETTINGS_YAWMODE_RATE=0, STABILIZATIONSETTINGS_YAWMODE_HEADING=1 } StabilizationSettingsYawModeOptions;
// Field ThrottleMax information // Field ThrottleMax information
// Field ThrottleMin information
// Field RollIntegralLimit information // Field RollIntegralLimit information
// Field PitchIntegralLimit information // Field PitchIntegralLimit information
// Field YawIntegralLimit information // Field YawIntegralLimit information

1
flight/OpenPilot/UAVObjects/stabilizationsettings.c
View File

@ -86,6 +86,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
data.YawMax = 35; data.YawMax = 35;
data.YawMode = 0; data.YawMode = 0;
data.ThrottleMax = 1; data.ThrottleMax = 1;
data.ThrottleMin = -1;
data.RollIntegralLimit = 0.5; data.RollIntegralLimit = 0.5;
data.PitchIntegralLimit = 0.5; data.PitchIntegralLimit = 0.5;
data.YawIntegralLimit = 0.5; data.YawIntegralLimit = 0.5;

9
ground/src/plugins/gcscontrol/gcscontrol.ui
View File

@ -7,7 +7,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>653</width> <width>653</width>
<height>290</height> <height>295</height>
</rect> </rect>
</property> </property>
<property name="sizePolicy"> <property name="sizePolicy">
@ -31,6 +31,13 @@
</property> </property>
</widget> </widget>
</item> </item>
<item>
<widget class="QCheckBox" name="checkBoxArmed">
<property name="text">
<string>Armed</string>
</property>
</widget>
</item>
<item> <item>
<widget class="QLabel" name="label"> <widget class="QLabel" name="label">
<property name="text"> <property name="text">

9
ground/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp
View File

@ -52,6 +52,10 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
// Set up slots and signals // Set up slots and signals
connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(gcsControlToggle(int))); connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(gcsControlToggle(int)));
connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(flightModeChanged(int))); connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(flightModeChanged(int)));
//connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(gcsArmedToggle(int)));
// Connect object updated event from UAVObject to also update check boxes
connect(getMCC(), SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*)));
} }
@ -71,6 +75,11 @@ ManualControlCommand* GCSControlGadgetWidget::getMCC()
return obj; return obj;
} }
void GCSControlGadgetWidget::mccChanged(UAVObject* obj)
{
m_gcscontrol->checkBoxArmed->setChecked(obj->getField("Armed")->getValue().toString() == "True");
}
/*! /*!
\brief Called when the gcs control is toggled and enabled or disables flight write access to manual control command \brief Called when the gcs control is toggled and enabled or disables flight write access to manual control command
*/ */

3
ground/src/plugins/gcscontrol/gcscontrolgadgetwidget.h
View File

@ -45,6 +45,9 @@ private slots:
void gcsControlToggle(int state); void gcsControlToggle(int state);
void flightModeChanged(int state); void flightModeChanged(int state);
protected slots:
void mccChanged(UAVObject*);
private: private:
Ui_GCSControl *m_gcscontrol; Ui_GCSControl *m_gcscontrol;
ManualControlCommand* getMCC(); ManualControlCommand* getMCC();

6
ground/src/plugins/uavobjects/manualcontrolcommand.cpp
View File

@ -48,6 +48,12 @@ ManualControlCommand::ManualControlCommand(): UAVDataObject(OBJID, ISSINGLEINST,
ConnectedEnumOptions.append("False"); ConnectedEnumOptions.append("False");
ConnectedEnumOptions.append("True"); ConnectedEnumOptions.append("True");
fields.append( new UAVObjectField(QString("Connected"), QString(""), UAVObjectField::ENUM, ConnectedElemNames, ConnectedEnumOptions) ); fields.append( new UAVObjectField(QString("Connected"), QString(""), UAVObjectField::ENUM, ConnectedElemNames, ConnectedEnumOptions) );
QStringList ArmedElemNames;
ArmedElemNames.append("0");
QStringList ArmedEnumOptions;
ArmedEnumOptions.append("False");
ArmedEnumOptions.append("True");
fields.append( new UAVObjectField(QString("Armed"), QString(""), UAVObjectField::ENUM, ArmedElemNames, ArmedEnumOptions) );
QStringList RollElemNames; QStringList RollElemNames;
RollElemNames.append("0"); RollElemNames.append("0");
fields.append( new UAVObjectField(QString("Roll"), QString("%"), UAVObjectField::FLOAT32, RollElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("Roll"), QString("%"), UAVObjectField::FLOAT32, RollElemNames, QStringList()) );

6
ground/src/plugins/uavobjects/manualcontrolcommand.h
View File

@ -44,6 +44,7 @@ public:
// Field structure // Field structure
typedef struct { typedef struct {
quint8 Connected; quint8 Connected;
quint8 Armed;
float Roll; float Roll;
float Pitch; float Pitch;
float Yaw; float Yaw;
@ -60,6 +61,9 @@ public:
// Field Connected information // Field Connected information
/* Enumeration options for field Connected */ /* Enumeration options for field Connected */
typedef enum { CONNECTED_FALSE=0, CONNECTED_TRUE=1 } ConnectedOptions; typedef enum { CONNECTED_FALSE=0, CONNECTED_TRUE=1 } ConnectedOptions;
// Field Armed information
/* Enumeration options for field Armed */
typedef enum { ARMED_FALSE=0, ARMED_TRUE=1 } ArmedOptions;
// Field Roll information // Field Roll information
// Field Pitch information // Field Pitch information
// Field Yaw information // Field Yaw information
@ -76,7 +80,7 @@ public:
// Constants // Constants
static const quint32 OBJID = 1289277596U; static const quint32 OBJID = 9594768U;
static const QString NAME; static const QString NAME;
static const bool ISSINGLEINST = 1; static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 0; static const bool ISSETTINGS = 0;

14
ground/src/plugins/uavobjects/manualcontrolcommand.py
View File

@ -49,6 +49,18 @@ _fields = [ \
'1' : 'True', '1' : 'True',
} }
), ),
uavobject.UAVObjectField(
'Armed',
'b',
1,
[
'0',
],
{
'0' : 'False',
'1' : 'True',
}
),
uavobject.UAVObjectField( uavobject.UAVObjectField(
'Roll', 'Roll',
'f', 'f',
@ -154,7 +166,7 @@ _fields = [ \
class ManualControlCommand(uavobject.UAVObject): class ManualControlCommand(uavobject.UAVObject):
## Object constants ## Object constants
OBJID = 1289277596 OBJID = 9594768
NAME = "ManualControlCommand" NAME = "ManualControlCommand"
METANAME = "ManualControlCommandMeta" METANAME = "ManualControlCommandMeta"
ISSINGLEINST = 1 ISSINGLEINST = 1

14
ground/src/plugins/uavobjects/stabilizationsettings.cpp
View File

@ -44,16 +44,16 @@ StabilizationSettings::StabilizationSettings(): UAVDataObject(OBJID, ISSINGLEINS
QList<UAVObjectField*> fields; QList<UAVObjectField*> fields;
QStringList UpdatePeriodElemNames; QStringList UpdatePeriodElemNames;
UpdatePeriodElemNames.append("0"); UpdatePeriodElemNames.append("0");
fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::UINT16, UpdatePeriodElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::UINT8, UpdatePeriodElemNames, QStringList()) );
QStringList RollMaxElemNames; QStringList RollMaxElemNames;
RollMaxElemNames.append("0"); RollMaxElemNames.append("0");
fields.append( new UAVObjectField(QString("RollMax"), QString("degrees"), UAVObjectField::FLOAT32, RollMaxElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("RollMax"), QString("degrees"), UAVObjectField::UINT8, RollMaxElemNames, QStringList()) );
QStringList PitchMaxElemNames; QStringList PitchMaxElemNames;
PitchMaxElemNames.append("0"); PitchMaxElemNames.append("0");
fields.append( new UAVObjectField(QString("PitchMax"), QString("degrees"), UAVObjectField::FLOAT32, PitchMaxElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("PitchMax"), QString("degrees"), UAVObjectField::UINT8, PitchMaxElemNames, QStringList()) );
QStringList YawMaxElemNames; QStringList YawMaxElemNames;
YawMaxElemNames.append("0"); YawMaxElemNames.append("0");
fields.append( new UAVObjectField(QString("YawMax"), QString("degrees"), UAVObjectField::FLOAT32, YawMaxElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("YawMax"), QString("degrees"), UAVObjectField::UINT8, YawMaxElemNames, QStringList()) );
QStringList YawModeElemNames; QStringList YawModeElemNames;
YawModeElemNames.append("0"); YawModeElemNames.append("0");
QStringList YawModeEnumOptions; QStringList YawModeEnumOptions;
@ -62,7 +62,10 @@ StabilizationSettings::StabilizationSettings(): UAVDataObject(OBJID, ISSINGLEINS
fields.append( new UAVObjectField(QString("YawMode"), QString("degrees"), UAVObjectField::ENUM, YawModeElemNames, YawModeEnumOptions) ); fields.append( new UAVObjectField(QString("YawMode"), QString("degrees"), UAVObjectField::ENUM, YawModeElemNames, YawModeEnumOptions) );
QStringList ThrottleMaxElemNames; QStringList ThrottleMaxElemNames;
ThrottleMaxElemNames.append("0"); ThrottleMaxElemNames.append("0");
fields.append( new UAVObjectField(QString("ThrottleMax"), QString("%"), UAVObjectField::FLOAT32, ThrottleMaxElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("ThrottleMax"), QString("frac"), UAVObjectField::FLOAT32, ThrottleMaxElemNames, QStringList()) );
QStringList ThrottleMinElemNames;
ThrottleMinElemNames.append("0");
fields.append( new UAVObjectField(QString("ThrottleMin"), QString("frac"), UAVObjectField::FLOAT32, ThrottleMinElemNames, QStringList()) );
QStringList RollIntegralLimitElemNames; QStringList RollIntegralLimitElemNames;
RollIntegralLimitElemNames.append("0"); RollIntegralLimitElemNames.append("0");
fields.append( new UAVObjectField(QString("RollIntegralLimit"), QString(""), UAVObjectField::FLOAT32, RollIntegralLimitElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("RollIntegralLimit"), QString(""), UAVObjectField::FLOAT32, RollIntegralLimitElemNames, QStringList()) );
@ -138,6 +141,7 @@ void StabilizationSettings::setDefaultFieldValues()
data.YawMax = 35; data.YawMax = 35;
data.YawMode = 0; data.YawMode = 0;
data.ThrottleMax = 1; data.ThrottleMax = 1;
data.ThrottleMin = -1;
data.RollIntegralLimit = 0.5; data.RollIntegralLimit = 0.5;
data.PitchIntegralLimit = 0.5; data.PitchIntegralLimit = 0.5;
data.YawIntegralLimit = 0.5; data.YawIntegralLimit = 0.5;

12
ground/src/plugins/uavobjects/stabilizationsettings.h
View File

@ -43,12 +43,13 @@ class UAVOBJECTS_EXPORT StabilizationSettings: public UAVDataObject
public: public:
// Field structure // Field structure
typedef struct { typedef struct {
quint16 UpdatePeriod; quint8 UpdatePeriod;
float RollMax; quint8 RollMax;
float PitchMax; quint8 PitchMax;
float YawMax; quint8 YawMax;
quint8 YawMode; quint8 YawMode;
float ThrottleMax; float ThrottleMax;
float ThrottleMin;
float RollIntegralLimit; float RollIntegralLimit;
float PitchIntegralLimit; float PitchIntegralLimit;
float YawIntegralLimit; float YawIntegralLimit;
@ -73,6 +74,7 @@ public:
/* Enumeration options for field YawMode */ /* Enumeration options for field YawMode */
typedef enum { YAWMODE_RATE=0, YAWMODE_HEADING=1 } YawModeOptions; typedef enum { YAWMODE_RATE=0, YAWMODE_HEADING=1 } YawModeOptions;
// Field ThrottleMax information // Field ThrottleMax information
// Field ThrottleMin information
// Field RollIntegralLimit information // Field RollIntegralLimit information
// Field PitchIntegralLimit information // Field PitchIntegralLimit information
// Field YawIntegralLimit information // Field YawIntegralLimit information
@ -88,7 +90,7 @@ public:
// Constants // Constants
static const quint32 OBJID = 1244337598U; static const quint32 OBJID = 117768092U;
static const QString NAME; static const QString NAME;
static const bool ISSINGLEINST = 1; static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1; static const bool ISSETTINGS = 1;

20
ground/src/plugins/uavobjects/stabilizationsettings.py
View File

@ -39,7 +39,7 @@ from collections import namedtuple
_fields = [ \ _fields = [ \
uavobject.UAVObjectField( uavobject.UAVObjectField(
'UpdatePeriod', 'UpdatePeriod',
'H', 'B',
1, 1,
[ [
'0', '0',
@ -49,7 +49,7 @@ _fields = [ \
), ),
uavobject.UAVObjectField( uavobject.UAVObjectField(
'RollMax', 'RollMax',
'f', 'B',
1, 1,
[ [
'0', '0',
@ -59,7 +59,7 @@ _fields = [ \
), ),
uavobject.UAVObjectField( uavobject.UAVObjectField(
'PitchMax', 'PitchMax',
'f', 'B',
1, 1,
[ [
'0', '0',
@ -69,7 +69,7 @@ _fields = [ \
), ),
uavobject.UAVObjectField( uavobject.UAVObjectField(
'YawMax', 'YawMax',
'f', 'B',
1, 1,
[ [
'0', '0',
@ -99,6 +99,16 @@ _fields = [ \
{ {
} }
), ),
uavobject.UAVObjectField(
'ThrottleMin',
'f',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField( uavobject.UAVObjectField(
'RollIntegralLimit', 'RollIntegralLimit',
'f', 'f',
@ -224,7 +234,7 @@ _fields = [ \
class StabilizationSettings(uavobject.UAVObject): class StabilizationSettings(uavobject.UAVObject):
## Object constants ## Object constants
OBJID = 1244337598 OBJID = 117768092
NAME = "StabilizationSettings" NAME = "StabilizationSettings"
METANAME = "StabilizationSettingsMeta" METANAME = "StabilizationSettingsMeta"
ISSINGLEINST = 1 ISSINGLEINST = 1

1
ground/src/shared/uavobjectdefinition/manualcontrolcommand.xml
View File

@ -2,6 +2,7 @@
<object name="ManualControlCommand" singleinstance="true" settings="false"> <object name="ManualControlCommand" singleinstance="true" settings="false">
<description>The output from the @ref ManualControlModule which descodes the receiver inputs. Overriden by GCS for fly-by-wire control.</description> <description>The output from the @ref ManualControlModule which descodes the receiver inputs. Overriden by GCS for fly-by-wire control.</description>
<field name="Connected" units="" type="enum" elements="1" options="False,True"/> <field name="Connected" units="" type="enum" elements="1" options="False,True"/>
<field name="Armed" units="" type="enum" elements="1" options="False,True"/>
<field name="Roll" units="%" type="float" elements="1"/> <field name="Roll" units="%" type="float" elements="1"/>
<field name="Pitch" units="%" type="float" elements="1"/> <field name="Pitch" units="%" type="float" elements="1"/>
<field name="Yaw" units="%" type="float" elements="1"/> <field name="Yaw" units="%" type="float" elements="1"/>

11
ground/src/shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -1,12 +1,13 @@
<xml> <xml>
<object name="StabilizationSettings" singleinstance="true" settings="true"> <object name="StabilizationSettings" singleinstance="true" settings="true">
<description>PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired</description> <description>PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired</description>
<field name="UpdatePeriod" units="ms" type="uint16" elements="1" defaultvalue="10"/> <field name="UpdatePeriod" units="ms" type="uint8" elements="1" defaultvalue="10"/>
<field name="RollMax" units="degrees" type="float" elements="1" defaultvalue="35"/> <field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="35"/>
<field name="PitchMax" units="degrees" type="float" elements="1" defaultvalue="35"/> <field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="35"/>
<field name="YawMax" units="degrees" type="float" elements="1" defaultvalue="35"/> <field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="35"/>
<field name="YawMode" units="degrees" type="enum" elements="1" options="rate,heading" defaultvalue="rate"/> <field name="YawMode" units="degrees" type="enum" elements="1" options="rate,heading" defaultvalue="rate"/>
<field name="ThrottleMax" units="%" type="float" elements="1" defaultvalue="1.0"/> <field name="ThrottleMax" units="frac" type="float" elements="1" defaultvalue="1.0"/>
<field name="ThrottleMin" units="frac" type="float" elements="1" defaultvalue="-1.0"/>
<field name="RollIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/> <field name="RollIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/>
<field name="PitchIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/> <field name="PitchIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/>
<field name="YawIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/> <field name="YawIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/>