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

OP-1197 configurable failsafes OP-1219 thrust control OP-1217 separate manualcontrolsettings apart

Browse Source
This commit is contained in:
Corvus Corax 2014-02-09 19:33:29 +01:00
parent c7ada40c2e
commit b5a27d05e7
35 changed files with 421 additions and 329 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
flight/libraries/sanitycheck.c
View File

@ -34,6 +34,7 @@
// UAVOs
#include <manualcontrolsettings.h>
#include <flightmodesettings.h>
#include <systemsettings.h>
#include <systemalarms.h>
#include <taskinfo.h>
@ -85,47 +86,47 @@ int32_t configuration_check()
// For each available flight mode position sanity check the available
// modes
uint8_t num_modes;
uint8_t modes[MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM];
uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes);
ManualControlSettingsFlightModePositionGet(modes);
FlightModeSettingsFlightModePositionGet(modes);
for (uint32_t i = 0; i < num_modes; i++) {
switch (modes[i]) {
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
if (multirotor) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
// TODO: put check equivalent to TASK_MONITOR_IsRunning
// here as soon as available for delayed callbacks
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@ -133,7 +134,7 @@ int32_t configuration_check()
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@ -141,7 +142,7 @@ int32_t configuration_check()
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@ -149,7 +150,7 @@ int32_t configuration_check()
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else {
@ -163,7 +164,7 @@ int32_t configuration_check()
}
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@ -201,23 +202,23 @@ int32_t configuration_check()
static int32_t check_stabilization_settings(int index, bool multirotor)
{
// Make sure the modes have identical sizes
if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM) {
if (FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) {
return SYSTEMALARMS_ALARM_ERROR;
}
uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM];
uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
// Get the different axis modes for this switch position
switch (index) {
case 1:
ManualControlSettingsStabilization1SettingsArrayGet(modes);
FlightModeSettingsStabilization1SettingsArrayGet(modes);
break;
case 2:
ManualControlSettingsStabilization2SettingsArrayGet(modes);
FlightModeSettingsStabilization2SettingsArrayGet(modes);
break;
case 3:
ManualControlSettingsStabilization3SettingsArrayGet(modes);
FlightModeSettingsStabilization3SettingsArrayGet(modes);
break;
default:
return SYSTEMALARMS_ALARM_ERROR;
@ -226,14 +227,14 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
// For multirotors verify that nothing is set to "none"
if (multirotor) {
for (uint32_t i = 0; i < NELEMENTS(modes); i++) {
if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) {
return SYSTEMALARMS_ALARM_ERROR;
}
}
}
// Warning: This assumes that certain conditions in the XML file are met. That
// MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE
return SYSTEMALARMS_ALARM_OK;

28
flight/modules/Actuator/actuator.c
View File

@ -166,6 +166,9 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
ActuatorDesiredData desired;
MixerStatusData mixerStatus;
FlightStatusData flightStatus;
SystemSettingsThrustControlOptions thrustType;
float throttleDesired;
float collectiveDesired;
/* Read initial values of ActuatorSettings */
ActuatorSettingsData actuatorSettings;
@ -220,6 +223,22 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
FlightStatusGet(&flightStatus);
ActuatorDesiredGet(&desired);
ActuatorCommandGet(&command);
SystemSettingsThrustControlGet(&thrustType);
// read in throttle and collective -demultiplex thrust
switch (thrustType) {
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
throttleDesired = desired.Thrust;
ManualControlCommandCollectiveGet(&collectiveDesired);
break;
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
ManualControlCommandThrottleGet(&throttleDesired);
collectiveDesired = desired.Thrust;
break;
default:
ManualControlCommandThrottleGet(&throttleDesired);
ManualControlCommandCollectiveGet(&collectiveDesired);
}
#ifdef DIAG_MIXERSTATUS
MixerStatusGet(&mixerStatus);
@ -239,17 +258,17 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool positiveThrottle = desired.Throttle >= 0.00f;
bool positiveThrottle = throttleDesired >= 0.00f;
bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
float curve1 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
// The source for the secondary curve is selectable
float curve2 = 0;
AccessoryDesiredData accessory;
switch (mixerSettings.Curve2Source) {
case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
curve2 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
curve2 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_ROLL:
curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
@ -262,8 +281,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
ManualControlCommandCollectiveGet(&curve2);
curve2 = MixerCurve(curve2, mixerSettings.ThrottleCurve2,
curve2 = MixerCurve(collectiveDesired, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:

28
flight/modules/AltitudeHold/altitudehold.c
View File

@ -114,7 +114,7 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
*/
static void altitudeHoldTask(void)
{
static float startThrottle = 0.5f;
static float startThrust = 0.5f;
// make sure we run only when we are supposed to run
FlightStatusData flightStatus;
@ -127,7 +127,7 @@ static void altitudeHoldTask(void)
default:
pid_zero(&pid0);
pid_zero(&pid1);
StabilizationDesiredThrottleGet(&startThrottle);
StabilizationDesiredThrustGet(&startThrust);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
@ -160,30 +160,30 @@ static void altitudeHoldTask(void)
AltitudeHoldStatusSet(&altitudeHoldStatus);
float throttle;
float thrust;
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE:
throttle = altitudeHoldDesired.SetPoint;
case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST:
thrust = altitudeHoldDesired.SetPoint;
break;
default:
// velocity control loop
throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
thrust = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
if (throttle >= 1.0f) {
throttle = 1.0f;
if (thrust >= 1.0f) {
thrust = 1.0f;
}
if (throttle <= 0.0f) {
throttle = 0.0f;
if (thrust <= 0.0f) {
thrust = 0.0f;
}
break;
}
StabilizationDesiredData stab;
StabilizationDesiredGet(&stab);
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Throttle = throttle;
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Thrust = thrust;
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;

8
flight/modules/Autotune/autotune.c
View File

@ -182,8 +182,8 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
stabDesired.Throttle = manualControl.Throttle;
stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
stabDesired.Thrust = manualControl.Thrust;
switch (state) {
case AT_INIT:
@ -191,7 +191,7 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
lastUpdateTime = xTaskGetTickCount();
// Only start when armed and flying
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) {
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Thrust > 0) {
state = AT_START;
}
break;
@ -236,7 +236,7 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
case AT_FINISHED:
// Wait until disarmed and landed before updating the settings
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) {
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Thrust <= 0) {
state = AT_SET;
}

30
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -357,10 +357,10 @@ static void updateFixedAttitude(float *attitude)
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
@ -420,7 +420,7 @@ static uint8_t updateFixedDesiredAttitude()
/**
* Compute speed error (required for throttle and pitch)
* Compute speed error (required for thrust and pitch)
*/
// Current ground speed
@ -474,9 +474,9 @@ static uint8_t updateFixedDesiredAttitude()
}
/**
* Compute desired throttle command
* Compute desired thrust command
*/
// compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant.
// compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
powerIntegral = bound(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
@ -491,7 +491,7 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
);
// Compute final throttle response
// Compute final thrust response
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp +
powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki +
speedErrorToPowerCommandComponent;
@ -501,14 +501,14 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral;
fixedwingpathfollowerStatus.Command.Power = powerCommand;
// set throttle
stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrottleLimit.Min,
fixedwingpathfollowerSettings.ThrottleLimit.Max);
// set thrust
stabDesired.Thrust = bound(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrustLimit.Min,
fixedwingpathfollowerSettings.ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors.Lowpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Max && // throttle at maximum
if (powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError > 0 && // we are too slow already
@ -516,9 +516,9 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.Errors.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedwingpathfollowerStatus.Errors.Highpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Min && // throttle at minimum
if (powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0 && // we ARE going up
descentspeedDesired > 0 && // we WANT to go down
airspeedError < 0 && // we are too fast already

56
flight/modules/ManualControl/inc/manualcontrol.h
View File

@ -61,46 +61,46 @@ int32_t ManualControlInitialize();
*/
#define assumptions1 \
( \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions3 \
( \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions5 \
( \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions_flightmode \
( \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \
)
#define assumptions_channelcount \

147
flight/modules/ManualControl/manualcontrol.c
View File

@ -44,6 +44,7 @@
#include "manualcontrol.h"
#include "manualcontrolsettings.h"
#include "manualcontrolcommand.h"
#include "flightmodesettings.h"
#include "positionstate.h"
#include "pathdesired.h"
#include "stabilizationbank.h"
@ -92,12 +93,12 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
// Private functions
static void updateActuatorDesired(ManualControlCommandData *cmd);
static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings);
static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings);
static void updateLandDesired(ManualControlCommandData *cmd, bool changed);
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed);
static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home);
static void processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd);
static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, int8_t armSwitch);
static void processFlightMode(ManualControlSettingsData *settings, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd);
static void processArm(ManualControlCommandData *cmd, FlightModeSettingsData *settings, int8_t armSwitch);
static void setArmedIfChanged(uint8_t val);
static void configurationUpdatedCb(UAVObjEvent *ev);
@ -157,6 +158,7 @@ int32_t ManualControlInitialize()
StabilizationDesiredInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
FlightModeSettingsInitialize();
return 0;
}
@ -168,6 +170,7 @@ MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
static void manualControlTask(__attribute__((unused)) void *parameters)
{
ManualControlSettingsData settings;
FlightModeSettingsData modeSettings;
ManualControlCommandData cmd;
FlightStatusData flightStatus;
float flightMode = 0;
@ -203,6 +206,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
lastSysTime = xTaskGetTickCount();
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 };
SystemSettingsThrustControlOptions thrustType;
while (1) {
// Wait until next update
@ -213,6 +217,8 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
// Read settings
ManualControlSettingsGet(&settings);
FlightModeSettingsGet(&modeSettings);
SystemSettingsThrustControlGet(&thrustType);
/* Update channel activity monitor */
if (flightStatus.Armed == ARM_STATE_DISARMED) {
@ -283,7 +289,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER ||
// Check the FlightModeNumber is valid
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
settings.FlightModeNumber < 1 || settings.FlightModeNumber > FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1)
&& (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
@ -323,16 +329,26 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
int8_t armSwitch = 0;
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 0;
cmd.Collective = 0;
if (settings.FailsafeBehavior != MANUALCONTROLSETTINGS_FAILSAFEBEHAVIOR_NONE) {
cmd.Throttle = settings.FailsafeChannel.Throttle;
cmd.Roll = settings.FailsafeChannel.Roll;
cmd.Pitch = settings.FailsafeChannel.Pitch;
cmd.Yaw = settings.FailsafeChannel.Yaw;
cmd.Collective = settings.FailsafeChannel.Collective;
switch (thrustType) {
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
cmd.Thrust = cmd.Throttle;
break;
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
cmd.Thrust = cmd.Collective;
break;
default:
break;
}
if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) {
FlightStatusGet(&flightStatus);
cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeBehavior - 1;
flightStatus.FlightMode = settings.FlightModePosition[settings.FailsafeBehavior - 1];
cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition;
flightStatus.FlightMode = modeSettings.FlightModePosition[settings.FailsafeFlightModeSwitchPosition];
FlightStatusSet(&flightStatus);
}
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
@ -340,21 +356,21 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
accessory.AccessoryVal = settings.FailsafeChannel.Accessory0;
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 1
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
accessory.AccessoryVal = settings.FailsafeChannel.Accessory1;
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 2
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
accessory.AccessoryVal = settings.FailsafeChannel.Accessory2;
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
@ -393,6 +409,17 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
}
switch (thrustType) {
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
cmd.Thrust = cmd.Throttle;
break;
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
cmd.Thrust = cmd.Collective;
break;
default:
break;
}
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
@ -400,7 +427,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY0) {
if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY0) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
@ -417,7 +444,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY1) {
if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY1) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
@ -434,7 +461,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY2) {
if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY2) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
@ -447,11 +474,11 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
}
}
processFlightMode(&settings, flightMode, &cmd);
processFlightMode(&settings, &modeSettings, flightMode, &cmd);
}
// Process arming outside conditional so system will disarm when disconnected
processArm(&cmd, &settings, armSwitch);
processArm(&cmd, &modeSettings, armSwitch);
// Update cmd object
ManualControlCommandSet(&cmd);
@ -481,7 +508,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
updateActuatorDesired(&cmd);
break;
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
updateStabilizationDesired(&cmd, &modeSettings);
break;
case FLIGHTMODE_TUNING:
// Tuning takes settings directly from manualcontrolcommand. No need to
@ -656,14 +683,14 @@ static void updateActuatorDesired(ManualControlCommandData *cmd)
ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator);
actuator.Roll = cmd->Roll;
actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
actuator.Roll = cmd->Roll;
actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw;
actuator.Thrust = cmd->Thrust;
ActuatorDesiredSet(&actuator);
}
static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings)
static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings)
{
StabilizationDesiredData stabilization;
@ -738,7 +765,7 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
0; // this is an invalid mode
}
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
stabilization.Thrust = cmd->Thrust;
StabilizationDesiredSet(&stabilization);
}
@ -761,8 +788,8 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
// Simple Return To Base mode - keep altitude the same, fly to home position
PositionStateData positionState;
PositionStateGet(&positionState);
ManualControlSettingsData settings;
ManualControlSettingsGet(&settings);
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
@ -850,9 +877,9 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// this is the max speed in m/s at the extents of throttle
float throttleRate;
uint8_t throttleExp;
// this is the max speed in m/s at the extents of thrust
float thrustRate;
uint8_t thrustExp;
static uint8_t flightMode;
static bool newaltitude = true;
@ -862,8 +889,8 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
AltitudeHoldSettingsThrustExpGet(&thrustExp);
AltitudeHoldSettingsThrustRateGet(&thrustRate);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
@ -880,20 +907,20 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
}
uint8_t cutOff;
AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff);
if (cutOff && cmd->Throttle < 0) {
// Cut throttle if desired
altitudeHoldDesiredData.SetPoint = cmd->Throttle;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE;
AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff);
if (cutOff && cmd->Thrust < 0) {
// Cut thrust if desired
altitudeHoldDesiredData.SetPoint = cmd->Thrust;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) {
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust > DEADBAND_HIGH) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
// then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
altitudeHoldDesiredData.SetPoint = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate);
altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd->Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd->Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) {
altitudeHoldDesiredData.SetPoint = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate);
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust < DEADBAND_LOW) {
altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd->Thrust < 0 ? 0 : cmd->Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd->Thrust) / DEADBAND_LOW) / 255 * thrustRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (newaltitude == true) {
@ -1041,7 +1068,7 @@ static void setArmedIfChanged(uint8_t val)
* @param[out] cmd The structure to set the armed in
* @param[in] settings Settings indicating the necessary position
*/
static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, int8_t armSwitch)
static void processArm(ManualControlCommandData *cmd, FlightModeSettingsData *settings, int8_t armSwitch)
{
bool lowThrottle = cmd->Throttle < 0;
@ -1049,9 +1076,9 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
* do NOT check throttle if disarming via switch, must be instant
*/
switch (settings->Arming) {
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2:
case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
if (armSwitch < 0) {
lowThrottle = true;
}
@ -1066,7 +1093,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
return;
}
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
if (settings->Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
@ -1093,7 +1120,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
}
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
if (settings->Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
@ -1106,27 +1133,27 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
// Calc channel see assumptions7
switch (settings->Arming) {
case MANUALCONTROLSETTINGS_ARMING_ROLLLEFT:
case FLIGHTMODESETTINGS_ARMING_ROLLLEFT:
armingInputLevel = 1.0f * cmd->Roll;
break;
case MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT:
case FLIGHTMODESETTINGS_ARMING_ROLLRIGHT:
armingInputLevel = -1.0f * cmd->Roll;
break;
case MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD:
case FLIGHTMODESETTINGS_ARMING_PITCHFORWARD:
armingInputLevel = 1.0f * cmd->Pitch;
break;
case MANUALCONTROLSETTINGS_ARMING_PITCHAFT:
case FLIGHTMODESETTINGS_ARMING_PITCHAFT:
armingInputLevel = -1.0f * cmd->Pitch;
break;
case MANUALCONTROLSETTINGS_ARMING_YAWLEFT:
case FLIGHTMODESETTINGS_ARMING_YAWLEFT:
armingInputLevel = 1.0f * cmd->Yaw;
break;
case MANUALCONTROLSETTINGS_ARMING_YAWRIGHT:
case FLIGHTMODESETTINGS_ARMING_YAWRIGHT:
armingInputLevel = -1.0f * cmd->Yaw;
break;
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2:
case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
armingInputLevel = -1.0f * (float)armSwitch;
break;
}
@ -1199,7 +1226,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
* @param[in] settings The settings which indicate which position is which mode
* @param[in] flightMode the value of the switch position
*/
static void processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd)
static void processFlightMode(ManualControlSettingsData *settings, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd)
{
FlightStatusData flightStatus;
@ -1213,7 +1240,7 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM
cmd->FlightModeSwitchPosition = pos;
uint8_t newMode = settings->FlightModePosition[pos];
uint8_t newMode = modeSettings->FlightModePosition[pos];
if (flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;

6
flight/modules/PathPlanner/pathplanner.c
View File

@ -41,7 +41,7 @@
#include "velocitystate.h"
#include "waypoint.h"
#include "waypointactive.h"
#include "manualcontrolsettings.h"
#include "flightmodesettings.h"
#include <pios_struct_helper.h>
#include "paths.h"
@ -170,8 +170,8 @@ static void pathPlannerTask()
// copy pasta: same calculation as in manualcontrol, set return to home coordinates
PositionStateData positionState;
PositionStateGet(&positionState);
ManualControlSettingsData settings;
ManualControlSettingsGet(&settings);
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
pathDesired.Start.North = 0;
pathDesired.Start.East = 0;

47
flight/modules/Stabilization/stabilization.c
View File

@ -49,6 +49,7 @@
#include "gyrostate.h"
#include "flightstatus.h"
#include "manualcontrolsettings.h"
#include "flightmodesettings.h"
#include "manualcontrol.h" // Just to get a macro
#include "taskinfo.h"
@ -110,8 +111,8 @@ struct pid pids[PID_MAX];
int cur_flight_mode = -1;
static uint8_t rattitude_anti_windup;
static float cruise_control_min_throttle;
static float cruise_control_max_throttle;
static float cruise_control_min_thrust;
static float cruise_control_max_thrust;
static uint8_t cruise_control_max_angle;
static float cruise_control_max_power_factor;
static float cruise_control_power_trim;
@ -168,7 +169,7 @@ int32_t StabilizationStart()
int32_t StabilizationInitialize()
{
// stop the compile if the number of switch positions changes, but has not been changed here
PIOS_STATIC_ASSERT(NUM_FMS_POSITIONS == sizeof(((ManualControlSettingsData *)0)->FlightModePosition) / sizeof((((ManualControlSettingsData *)0)->FlightModePosition)[0]));
PIOS_STATIC_ASSERT(NUM_FMS_POSITIONS == sizeof(((FlightModeSettingsData *)0)->FlightModePosition) / sizeof((((FlightModeSettingsData *)0)->FlightModePosition)[0]));
// Initialize variables
StabilizationSettingsInitialize();
@ -205,6 +206,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
float throttleDesired;
RateDesiredData rateDesired;
AttitudeStateData attitudeState;
GyroStateData gyroStateData;
@ -236,6 +238,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
ManualControlCommandThrottleGet(&throttleDesired);
AttitudeStateGet(&attitudeState);
GyroStateGet(&gyroStateData);
StabilizationBankGet(&stabBank);
@ -592,10 +595,10 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Throttle = stabDesired.Throttle;
actuatorDesired.Thrust = stabDesired.Thrust;
// Suppress desired output while disarmed or throttle low, for configured axis
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || stabDesired.Throttle < 0) {
// Suppress desired output while disarmed or thrust low, for configured axis
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || throttleDesired < 0) {
if (lowThrottleZeroAxis[ROLL]) {
actuatorDesired.Roll = 0.0f;
}
@ -609,9 +612,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
}
// modify throttle according to 1/cos(bank angle)
// modify thrust according to 1/cos(bank angle)
// to maintain same altitdue with changing bank angle
// but without manually adjusting throttle
// but without manually adjusting thrust
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
if (flight_mode_switch_position < NUM_FMS_POSITIONS
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
@ -645,7 +648,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// if inverted and they want negative boost
if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t)-1) {
factor = -factor;
// as long as throttle is getting reversed
// as long as thrust is getting reversed
// we may as well do pitch and yaw for a complete "invert switch"
actuatorDesired.Pitch = -actuatorDesired.Pitch;
actuatorDesired.Yaw = -actuatorDesired.Yaw;
@ -653,15 +656,15 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
}
// also don't adjust throttle if <= 0, leaves neg alone and zero throttle stops motors
if (actuatorDesired.Throttle > cruise_control_min_throttle) {
// also don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors
if (actuatorDesired.Thrust > cruise_control_min_thrust) {
// quad example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8
// CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7
actuatorDesired.Throttle = (actuatorDesired.Throttle - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
if (actuatorDesired.Throttle > cruise_control_max_throttle) {
actuatorDesired.Throttle = cruise_control_max_throttle;
} else if (actuatorDesired.Throttle < cruise_control_min_throttle) {
actuatorDesired.Throttle = cruise_control_min_throttle;
actuatorDesired.Thrust = (actuatorDesired.Thrust - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
if (actuatorDesired.Thrust > cruise_control_max_thrust) {
actuatorDesired.Thrust = cruise_control_max_thrust;
} else if (actuatorDesired.Thrust < cruise_control_min_thrust) {
actuatorDesired.Thrust = cruise_control_min_thrust;
}
}
}
@ -676,7 +679,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0)) {
(lowThrottleZeroIntegral && throttleDesired < 0)) {
// Force all axes to reinitialize when engaged
for (uint8_t i = 0; i < MAX_AXES; i++) {
previous_mode[i] = 255;
@ -906,10 +909,10 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
weak_leveling_kp = settings.WeakLevelingKp;
weak_leveling_max = settings.MaxWeakLevelingRate;
// Whether to zero the PID integrals while throttle is low
// Whether to zero the PID integrals while thrust is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low
// Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or thrust is low
lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
@ -933,10 +936,10 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
cur_flight_mode = -1;
// Rattitude flight mode anti-windup factor
rattitude_anti_windup = settings.RattitudeAntiWindup;
rattitude_anti_windup = settings.RattitudeAntiWindup;
cruise_control_min_throttle = (float)settings.CruiseControlMinThrottle / 100.0f;
cruise_control_max_throttle = (float)settings.CruiseControlMaxThrottle / 100.0f;
cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f;
cruise_control_max_thrust = (float)settings.CruiseControlMaxThrust / 100.0f;
cruise_control_max_angle = settings.CruiseControlMaxAngle;
cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor;
cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f;

32
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -158,7 +158,7 @@ static float northPosIntegral = 0;
static float eastPosIntegral = 0;
static float downPosIntegral = 0;
static float throttleOffset = 0;
static float thrustOffset = 0;
/**
* Module thread, should not return.
*/
@ -263,10 +263,10 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
eastPosIntegral = 0;
downPosIntegral = 0;
// Track throttle before engaging this mode. Cheap system ident
// Track thrust before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
thrustOffset = stabDesired.Thrust;
break;
}
@ -550,10 +550,10 @@ static void updateFixedAttitude(float *attitude)
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
@ -653,13 +653,13 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
downError = velocityDesired.Down - downVel;
// Must flip this sign
downError = -downError;
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
vtolpathfollowerSettings.VerticalVelPID.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
vtolpathfollowerSettings.VerticalVelPID.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
stabDesired.Thrust = bound(downCommand + thrustOffset, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
@ -670,11 +670,11 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) {
// For now override thrust with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Throttle = manualControl.Throttle;
stabDesired.Thrust = manualControl.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;

1
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -98,6 +98,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c
SRC += $(OPUAVSYNTHDIR)/flightmodesettings.c
SRC += $(OPUAVSYNTHDIR)/mixersettings.c
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
SRC += $(OPUAVSYNTHDIR)/attitudesettings.c

1
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -60,6 +60,7 @@ UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += flightmodesettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel

1
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -61,6 +61,7 @@ UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += flightmodesettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel

1
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -65,6 +65,7 @@ UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += flightmodesettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel

69
ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
View File

@ -57,10 +57,11 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
loop(NULL),
skipflag(false)
{
manualCommandObj = ManualControlCommand::GetInstance(getObjectManager());
manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager());
flightStatusObj = FlightStatus::GetInstance(getObjectManager());
receiverActivityObj = ReceiverActivity::GetInstance(getObjectManager());
manualCommandObj = ManualControlCommand::GetInstance(getObjectManager());
manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager());
flightModeSettingsObj = FlightModeSettings::GetInstance(getObjectManager());
flightStatusObj = FlightStatus::GetInstance(getObjectManager());
receiverActivityObj = ReceiverActivity::GetInstance(getObjectManager());
ui = new Ui_InputWidget();
ui->setupUi(this);
@ -132,26 +133,26 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
connect(ui->wzBack, SIGNAL(clicked()), this, SLOT(wzBack()));
ui->stackedWidget->setCurrentIndex(0);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos2, 1, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos3, 2, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos4, 3, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos5, 4, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos6, 5, 1, true);
addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true);
addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos2, 1, 1, true);
addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos3, 2, 1, true);
addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos4, 3, 1, true);
addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos5, 4, 1, true);
addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos6, 5, 1, true);
addWidgetBinding("ManualControlSettings", "FlightModeNumber", ui->fmsPosNum);
addWidgetBinding("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Roll, "Roll", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Roll, "Roll", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Roll, "Roll", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Pitch, "Pitch", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Pitch, "Pitch", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Pitch, "Pitch", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Yaw, "Yaw", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true);
addWidgetBinding("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Roll, "Roll", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Roll, "Roll", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Roll, "Roll", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Pitch, "Pitch", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Pitch, "Pitch", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Pitch, "Pitch", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Yaw, "Yaw", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true);
addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true);
addWidgetBinding("ManualControlSettings", "Arming", ui->armControl);
addWidgetBinding("ManualControlSettings", "ArmedTimeout", ui->armTimeout, 0, 1000);
addWidgetBinding("FlightModeSettings", "Arming", ui->armControl);
addWidgetBinding("FlightModeSettings", "ArmedTimeout", ui->armTimeout, 0, 1000);
connect(ManualControlCommand::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveFMSlider()));
connect(ManualControlSettings::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updatePositionSlider()));
@ -365,10 +366,12 @@ void ConfigInputWidget::goToWizard()
// that the UAVO is changed, but then backs out to the start and
// chooses a different TX type (which could otherwise result in
// unexpected TX channels being enabled)
manualSettingsData = manualSettingsObj->getData();
previousManualSettingsData = manualSettingsData;
manualSettingsData.Arming = ManualControlSettings::ARMING_ALWAYSDISARMED;
manualSettingsObj->setData(manualSettingsData);
manualSettingsData = manualSettingsObj->getData();
previousManualSettingsData = manualSettingsData;
flightModeSettingsData = flightModeSettingsObj->getData();
previousFlightModeSettingsData = flightModeSettingsData;
flightModeSettingsData.Arming = FlightModeSettings::ARMING_ALWAYSDISARMED;
flightModeSettingsObj->setData(flightModeSettingsData);
// start the wizard
wizardSetUpStep(wizardWelcome);
@ -398,6 +401,7 @@ void ConfigInputWidget::wzCancel()
// Load settings back from beginning of wizard
manualSettingsObj->setData(previousManualSettingsData);
flightModeSettingsObj->setData(previousFlightModeSettingsData);
}
void ConfigInputWidget::wzNext()
@ -1218,13 +1222,13 @@ void ConfigInputWidget::moveSticks()
Q_ASSERT(0);
break;
}
if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[0]) {
if (flightStatusData.FlightMode == flightModeSettingsData.FlightModePosition[0]) {
m_txFlightMode->setElementId("flightModeLeft");
m_txFlightMode->setTransform(m_txFlightModeLOrig, false);
} else if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[1]) {
} else if (flightStatusData.FlightMode == flightModeSettingsData.FlightModePosition[1]) {
m_txFlightMode->setElementId("flightModeCenter");
m_txFlightMode->setTransform(m_txFlightModeCOrig, false);
} else if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[2]) {
} else if (flightStatusData.FlightMode == flightModeSettingsData.FlightModePosition[2]) {
m_txFlightMode->setElementId("flightModeRight");
m_txFlightMode->setTransform(m_txFlightModeROrig, false);
}
@ -1418,11 +1422,12 @@ void ConfigInputWidget::simpleCalibration(bool enable)
msgBox.setDefaultButton(QMessageBox::Ok);
msgBox.exec();
manualCommandData = manualCommandObj->getData();
manualCommandData = manualCommandObj->getData();
manualSettingsData = manualSettingsObj->getData();
manualSettingsData.Arming = ManualControlSettings::ARMING_ALWAYSDISARMED;
manualSettingsObj->setData(manualSettingsData);
manualSettingsData = manualSettingsObj->getData();
flightModeSettingsData = flightModeSettingsObj->getData();
flightModeSettingsData.Arming = FlightModeSettings::ARMING_ALWAYSDISARMED;
flightModeSettingsObj->setData(flightModeSettingsData);
for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) {
reverse[i] = manualSettingsData.ChannelMax[i] < manualSettingsData.ChannelMin[i];

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

@ -39,6 +39,7 @@
#include <QRadioButton>
#include "manualcontrolcommand.h"
#include "manualcontrolsettings.h"
#include "flightmodesettings.h"
#include "receiveractivity.h"
#include <QGraphicsView>
#include <QtSvg/QSvgRenderer>
@ -109,6 +110,9 @@ private:
ManualControlSettings *manualSettingsObj;
ManualControlSettings::DataFields manualSettingsData;
ManualControlSettings::DataFields previousManualSettingsData;
FlightModeSettings *flightModeSettingsObj;
FlightModeSettings::DataFields flightModeSettingsData;
FlightModeSettings::DataFields previousFlightModeSettingsData;
ReceiverActivity *receiverActivityObj;
ReceiverActivity::DataFields receiverActivityData;

26
ground/openpilotgcs/src/plugins/config/stabilization.ui
View File

@ -27160,7 +27160,7 @@ border-radius: 5;</string>
<item row="1" column="4">
<widget class="QDoubleSpinBox" name="doubleSpinBox_6">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;-1, 0, or 1. Cruise Control multiplies the throttle stick by this value if the bank angle is past MaxAngle. The default is 0 which says to turn the motors off (actually set them to MinThrottle) when inverted. 1 says to use the unboosted throttle stick value. -1 (DON'T USE, INCOMPLETE, UNTESTED, for use by CP helis using idle up) says to reverse the throttle stick when inverted.
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;-1, 0, or 1. Cruise Control multiplies the throttle/collective stick by this value if the bank angle is past MaxAngle. The default is 0 which says to turn the motors off (actually set them to MinThrust) when inverted. 1 says to use the unboosted stick value. -1 (DON'T USE, INCOMPLETE, UNTESTED, for use by CP helis using idle up) says to reverse the collective stick when inverted.
&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
@ -27236,7 +27236,7 @@ border-radius: 5;</string>
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Really just a safety limit. 4.0 means it will not use more than 4 times the power the throttle stick is requesting.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Really just a safety limit. 4.0 means it will not use more than 4 times the power the throttle/collective stick is requesting.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -27457,7 +27457,7 @@ color: rgb(255, 255, 255);
border-radius: 5;</string>
</property>
<property name="text">
<string>MinThrottle</string>
<string>MinThrust</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@ -27467,7 +27467,7 @@ border-radius: 5;</string>
<item row="1" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBox_4">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Throttle stick below this disables Cruise Control. Also, by default Cruise Control forces the use of this value for throttle when the copter is inverted. For safety, never set this so low that the trimmed throttle stick cannot get below it.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Throttle/Collective stick below this disables Cruise Control. Also, by default Cruise Control forces the use of this value for thrust when the copter is inverted. For safety, never set this so low that the trimmed throttle/collective stick cannot get below it.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@ -27484,7 +27484,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlMinThrottle</string>
<string>fieldname:CruiseControlMinThrust</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
@ -27545,7 +27545,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:StabilizationSettings</string>
<string>fieldname:CruiseControlMaxThrottle</string>
<string>fieldname:CruiseControlMaxThrust</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:16</string>
@ -27579,7 +27579,7 @@ color: rgb(255, 255, 255);
border-radius: 5;</string>
</property>
<property name="text">
<string>MaxThrottle</string>
<string>MaxThrust</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@ -30695,7 +30695,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:ThrottleRate</string>
<string>fieldname:ThrustRate</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:99</string>
@ -30768,7 +30768,7 @@ border-radius: 5;</string>
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Throttle exponential value.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Thrust exponential value.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="styleSheet">
<string notr="true"/>
@ -30791,7 +30791,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:ThrottleExp</string>
<string>fieldname:ThrustExp</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:99,10</string>
@ -30865,7 +30865,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:ThrottleRate</string>
<string>fieldname:ThrustRate</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:99,10</string>
@ -30890,7 +30890,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:ThrottleExp</string>
<string>fieldname:ThrustExp</string>
<string>haslimits:no</string>
<string>scale:1</string>
<string>buttongroup:99</string>
@ -31441,7 +31441,7 @@ color: rgb(255, 255, 255);
border-radius: 5;</string>
</property>
<property name="text">
<string>Throttle Stick Response</string>
<string>Throttle/Collective Stick Response</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>

2
ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp
View File

@ -97,7 +97,7 @@ void AeroSimRCSimulator::transmitUpdate()
roll = actData.Roll;
pitch = -actData.Pitch;
yaw = actData.Yaw;
throttle = (actData.Throttle * 2.0) - 1.0;
throttle = (actData.Thrust * 2.0) - 1.0;
}
channels[0] = roll;
channels[1] = pitch;

10
ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp
View File

@ -174,7 +174,7 @@ void FGSimulator::transmitUpdate()
ailerons = actData.Roll;
elevator = -actData.Pitch;
rudder = actData.Yaw;
throttle = actData.Throttle;
throttle = actData.Thrust;
}
int allowableDifference = 10;
@ -209,10 +209,10 @@ void FGSimulator::transmitUpdate()
}
if (settings.manualControlEnabled) {
actData.Roll = ailerons;
actData.Pitch = -elevator;
actData.Yaw = rudder;
actData.Throttle = throttle;
actData.Roll = ailerons;
actData.Pitch = -elevator;
actData.Yaw = rudder;
actData.Thrust = throttle;
// actData.NumLongUpdates = (float)udpCounterFGrecv;
// actData.UpdateTime = (float)udpCounterGCSsend;
actDesired->setData(actData);

2
ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp
View File

@ -104,7 +104,7 @@ void IL2Simulator::transmitUpdate()
float ailerons = actData.Roll;
float elevator = actData.Pitch;
float rudder = actData.Yaw;
float throttle = actData.Throttle * 2 - 1.0;
float throttle = actData.Thrust * 2 - 1.0;
// Send update to Il2
QString cmd;

2
ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp
View File

@ -102,7 +102,7 @@ void XplaneSimulator::transmitUpdate()
float ailerons = actData.Roll;
float elevator = actData.Pitch;
float rudder = actData.Yaw;
float throttle = actData.Throttle > 0 ? actData.Throttle : 0;
float throttle = actData.Thrust > 0 ? actData.Thrust : 0;
float none = -999;
// quint32 none = *((quint32*)&tmp); // get float as 4 bytes

2
ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp
View File

@ -126,7 +126,7 @@ void opmap_edit_waypoint_dialog::setupModeWidgets()
ui->modeParam1->setText("pitch");
ui->modeParam2->setText("roll");
ui->modeParam3->setText("yaw");
ui->modeParam4->setText("throttle");
ui->modeParam4->setText("thrust");
ui->modeParam1->setVisible(true);
ui->modeParam2->setVisible(true);
ui->modeParam3->setVisible(true);

44
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
View File

@ -33,6 +33,7 @@
#include "mixersettings.h"
#include "systemsettings.h"
#include "manualcontrolsettings.h"
#include "flightmodesettings.h"
#include "stabilizationsettings.h"
#include "revocalibration.h"
@ -305,29 +306,34 @@ void VehicleConfigurationHelper::applyActuatorConfiguration()
void VehicleConfigurationHelper::applyFlighModeConfiguration()
{
FlightModeSettings *modeSettings = FlightModeSettings::GetInstance(m_uavoManager);
ManualControlSettings *controlSettings = ManualControlSettings::GetInstance(m_uavoManager);
Q_ASSERT(modeSettings);
Q_ASSERT(controlSettings);
ManualControlSettings::DataFields data = controlSettings->getData();
data.Stabilization1Settings[0] = ManualControlSettings::STABILIZATION1SETTINGS_ATTITUDE;
data.Stabilization1Settings[1] = ManualControlSettings::STABILIZATION1SETTINGS_ATTITUDE;
data.Stabilization1Settings[2] = ManualControlSettings::STABILIZATION1SETTINGS_AXISLOCK;
data.Stabilization2Settings[0] = ManualControlSettings::STABILIZATION2SETTINGS_ATTITUDE;
data.Stabilization2Settings[1] = ManualControlSettings::STABILIZATION2SETTINGS_ATTITUDE;
data.Stabilization2Settings[2] = ManualControlSettings::STABILIZATION2SETTINGS_RATE;
data.Stabilization3Settings[0] = ManualControlSettings::STABILIZATION3SETTINGS_RATE;
data.Stabilization3Settings[1] = ManualControlSettings::STABILIZATION3SETTINGS_RATE;
data.Stabilization3Settings[2] = ManualControlSettings::STABILIZATION3SETTINGS_RATE;
data.FlightModeNumber = 3;
data.FlightModePosition[0] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1;
data.FlightModePosition[1] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED2;
data.FlightModePosition[2] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED3;
data.FlightModePosition[3] = ManualControlSettings::FLIGHTMODEPOSITION_ALTITUDEHOLD;
data.FlightModePosition[4] = ManualControlSettings::FLIGHTMODEPOSITION_POSITIONHOLD;
data.FlightModePosition[5] = ManualControlSettings::FLIGHTMODEPOSITION_MANUAL;
controlSettings->setData(data);
addModifiedObject(controlSettings, tr("Writing flight mode settings"));
FlightModeSettings::DataFields data = modeSettings->getData();
ManualControlSettings::DataFields data2 = controlSettings->getData();
data.Stabilization1Settings[0] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE;
data.Stabilization1Settings[1] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE;
data.Stabilization1Settings[2] = FlightModeSettings::STABILIZATION1SETTINGS_AXISLOCK;
data.Stabilization2Settings[0] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE;
data.Stabilization2Settings[1] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE;
data.Stabilization2Settings[2] = FlightModeSettings::STABILIZATION2SETTINGS_RATE;
data.Stabilization3Settings[0] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
data.Stabilization3Settings[1] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
data.Stabilization3Settings[2] = FlightModeSettings::STABILIZATION3SETTINGS_RATE;
data2.FlightModeNumber = 3;
data.FlightModePosition[0] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED1;
data.FlightModePosition[1] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED2;
data.FlightModePosition[2] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED3;
data.FlightModePosition[3] = FlightModeSettings::FLIGHTMODEPOSITION_ALTITUDEHOLD;
data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_POSITIONHOLD;
data.FlightModePosition[5] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL;
modeSettings->setData(data);
addModifiedObject(modeSettings, tr("Writing flight mode settings 1/2"));
controlSettings->setData(data2);
addModifiedObject(controlSettings, tr("Writing flight mode settings 2/2"));
}
void VehicleConfigurationHelper::applySensorBiasConfiguration()

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

@ -71,6 +71,7 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/stabilizationbank.h \
$$UAVOBJECT_SYNTHETICS/manualcontrolsettings.h \
$$UAVOBJECT_SYNTHETICS/manualcontrolcommand.h \
$$UAVOBJECT_SYNTHETICS/flightmodesettings.h \
$$UAVOBJECT_SYNTHETICS/stabilizationdesired.h \
$$UAVOBJECT_SYNTHETICS/actuatorsettings.h \
$$UAVOBJECT_SYNTHETICS/actuatordesired.h \
@ -169,6 +170,7 @@ SOURCES += \
$$UAVOBJECT_SYNTHETICS/stabilizationbank.cpp \
$$UAVOBJECT_SYNTHETICS/manualcontrolsettings.cpp \
$$UAVOBJECT_SYNTHETICS/manualcontrolcommand.cpp \
$$UAVOBJECT_SYNTHETICS/flightmodesettings.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationdesired.cpp \
$$UAVOBJECT_SYNTHETICS/actuatorsettings.cpp \
$$UAVOBJECT_SYNTHETICS/actuatordesired.cpp \

2
shared/uavobjectdefinition/actuatordesired.xml
View File

@ -4,7 +4,7 @@
<field name="Roll" units="%" type="float" elements="1"/>
<field name="Pitch" units="%" type="float" elements="1"/>
<field name="Yaw" units="%" type="float" elements="1"/>
<field name="Throttle" units="%" type="float" elements="1"/>
<field name="Thrust" units="%" type="float" elements="1"/>
<field name="UpdateTime" units="ms" type="float" elements="1"/>
<field name="NumLongUpdates" units="ms" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>

2
shared/uavobjectdefinition/altitudeholddesired.xml
View File

@ -2,7 +2,7 @@
<object name="AltitudeHoldDesired" singleinstance="true" settings="false" category="Control">
<description>Holds the desired altitude (from manual control) as well as the desired attitude to pass through</description>
<field name="SetPoint" units="" type="float" elements="1"/>
<field name="ControlMode" units="" type="enum" elements="1" options="Altitude,Velocity,Throttle" />
<field name="ControlMode" units="" type="enum" elements="1" options="Altitude,Velocity,Thrust" />
<field name="Roll" units="deg" type="float" elements="1"/>
<field name="Pitch" units="deg" type="float" elements="1"/>
<field name="Yaw" units="deg/s" type="float" elements="1"/>

6
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -3,9 +3,9 @@
<description>Settings for the @ref AltitudeHold module</description>
<field name="AltitudePI" units="(m/s)/m" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.8,0,0" />
<field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.2,0.0002,2.0" />
<field name="CutThrottleWhenZero" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" />
<field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128" />
<field name="ThrottleRate" units="m/s" type="float" elements="1" defaultvalue="5" />
<field name="CutThrustWhenZero" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" />
<field name="ThrustExp" units="" type="uint8" elements="1" defaultvalue="128" />
<field name="ThrustRate" units="m/s" type="float" elements="1" defaultvalue="5" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

6
shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
View File

@ -32,7 +32,7 @@
<!-- proportional coefficient for adjusting vertical speed error for power calculation
in relation to airspeed error IASerror -->
<field name="PowerPI" units="1/(m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.01,0.05,0.5"/>
<!-- proportional coefficient for desired throttle
<!-- proportional coefficient for desired thrust
in relation to vertical speed error (absolute but including crossfeed) -->
<!-- output limits -->
@ -40,8 +40,8 @@
<!-- maximum allowed bank angles in navigates flight -->
<field name="PitchLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-10,5,20" />
<!-- maximum allowed pitch angles and setpoint for neutral pitch -->
<field name="ThrottleLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" />
<!-- minimum and maximum allowed throttle and setpoint for cruise speed -->
<field name="ThrustLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" />
<!-- minimum and maximum allowed thrust and setpoint for cruise speed -->
<field name="Safetymargins" units="" type="float"
elementnames="Wind, Stallspeed, Lowspeed, Highspeed, Overspeed, Lowpower, Highpower, Pitchcontrol"
defaultvalue="90, 1.0, 0.5, 1.5, 1.0, 1, 1, 1" />

66
shared/uavobjectdefinition/flightmodesettings.xml Normal file
View File

@ -0,0 +1,66 @@
<xml>
<object name="FlightModeSettings" singleinstance="true" settings="true" category="Control">
<description>Settings to control arming and flight mode</description>
<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,Accessory 0,Accessory 1,Accessory 2" 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,Rattitude,RelayRate,RelayAttitude"
defaultvalue="Attitude,Attitude,AxisLock"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
<field name="Stabilization2Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw"
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
defaultvalue="Attitude,Attitude,Rate"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
<field name="Stabilization3Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw"
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
defaultvalue="Rate,Rate,Rate"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
<!-- Note these options values should be identical to those defined in FlightMode -->
<!-- Currently only some modes are enabled for UI using limits attribute per board. Update when more modes will be operational -->
<field name="FlightModePosition"
units=""
type="enum"
elements="6"
options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"
defaultvalue="Stabilized1,Stabilized2,Stabilized3,AltitudeHold,AltitudeVario,Manual"
limits="\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="ReturnToHomeAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
<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>

1
shared/uavobjectdefinition/manualcontrolcommand.xml
View File

@ -7,6 +7,7 @@
<field name="Pitch" units="%" type="float" elements="1"/>
<field name="Yaw" units="%" type="float" elements="1"/>
<field name="Collective" units="%" type="float" elements="1"/>
<field name="Thrust" units="%" type="float" elements="1"/>
<field name="Channel" units="us" type="uint16" elements="9"/>
<field name="FlightModeSwitchPosition" units="" type="uint8" elements="1" defaultvalue="0"/>
<access gcs="readwrite" flight="readwrite"/>

57
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -17,64 +17,13 @@
<field name="Deadband" units="%" type="float" elements="1" defaultvalue="0"/>
<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,Accessory 0,Accessory 1,Accessory 2" 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,Rattitude,RelayRate,RelayAttitude"
defaultvalue="Attitude,Attitude,AxisLock"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
<field name="Stabilization2Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw"
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
defaultvalue="Attitude,Attitude,Rate"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
<field name="Stabilization3Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw"
options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
defaultvalue="Rate,Rate,Rate"
limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
<!-- Note these options values should be identical to those defined in FlightMode -->
<field name="FlightModeNumber" units="" type="uint8" elements="1" defaultvalue="3"/>
<!-- Currently only some modes are enabled for UI using limits attribute per board. Update when more modes will be operational -->
<field name="FlightModePosition"
units=""
type="enum"
elements="6"
options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"
defaultvalue="Stabilized1,Stabilized2,Stabilized3,AltitudeHold,AltitudeVario,Manual"
limits="\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
\
%0401NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0402NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="FailsafeBehavior" units="" type="enum" elements="1" options="None,ModePos1,ModePos2,ModePos3,ModePos4,ModePos5,ModePos6" defaultvalue="None"/>
<field name="ReturnToHomeAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
<field name="FailsafeFlightModeSwitchPosition" units="" type="int8" elements="1" defaultvalue="-1"/>
<field name="FailsafeChannel" units="%" type="float" elementnames="Throttle,Roll,Pitch,Yaw,Collective,Accessory0,Accessory1,Accessory2" defaultvalue="-1,0,0,0,0,0,0,0" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

3
shared/uavobjectdefinition/stabilizationdesired.xml
View File

@ -4,9 +4,10 @@
<field name="Roll" units="degrees" type="float" elements="1"/>
<field name="Pitch" units="degrees" type="float" elements="1"/>
<field name="Yaw" units="degrees" type="float" elements="1"/>
<field name="Throttle" units="%" type="float" elements="1"/>
<field name="Thrust" 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,Rattitude,RelayRate,RelayAttitude"/>
<field name="ThrustStabilizationMode" units="" type="enum" elements="1" options="None"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

4
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -30,8 +30,8 @@
<field name="RattitudeAntiWindup" units="" type="uint8" elements="1" defaultvalue="10"/>
<field name="CruiseControlMinThrottle" units="%" type="uint8" elements="1" defaultvalue="5"/>
<field name="CruiseControlMaxThrottle" units="%" type="uint8" elements="1" defaultvalue="90"/>
<field name="CruiseControlMinThrust" units="%" type="uint8" elements="1" defaultvalue="5"/>
<field name="CruiseControlMaxThrust" units="%" type="uint8" elements="1" defaultvalue="90"/>
<field name="CruiseControlMaxAngle" units="deg" type="uint8" elements="1" defaultvalue="105"/>
<field name="CruiseControlMaxPowerFactor" units="x" type="float" elements="1" defaultvalue="3.0"/>
<field name="CruiseControlPowerTrim" units="%" type="float" elements="1" defaultvalue="100.0"/>

5
shared/uavobjectdefinition/systemsettings.xml
View File

@ -2,6 +2,11 @@
<object name="SystemSettings" singleinstance="true" settings="true" category="System">
<description>Select airframe type. Currently used by @ref ActuatorModule to choose mixing from @ref ActuatorDesired to @ref ActuatorCommand</description>
<field name="AirframeType" units="" type="enum" elements="1" options="FixedWing,FixedWingElevon,FixedWingVtail,VTOL,HeliCP,QuadX,QuadP,Hexa,Octo,Custom,HexaX,OctoV,OctoCoaxP,OctoCoaxX,HexaCoax,Tri,GroundVehicleCar,GroundVehicleDifferential,GroundVehicleMotorcycle" defaultvalue="QuadX"/>
<field name="ThrustControl" units="" type="enum" elements="1" options="Throttle,Collective,None" defaultvalue="Throttle" />
<!-- Which way the vehicle controls its thrust. Can be through
"Throttle" (quadcopter, simple brushless planes, car-type ground vehicles)
"Collective" (collective pitch as in most helicopters, 3d quads, constant RPM variable pitch airplanes, and ground vehicles with diesel-electric locomotion)
"None" (craft has neither engines nor dynamic brakes. Note that a glider with breaking flaps should likely use "collective" and use the collective channel to control the brakes for optimum autopilot performance) -->
<field name="GUIConfigData" units="bits" type="uint32" elements="4" defaultvalue="0"/>
<field name="AirSpeedMax" units="m/s" type="float" elements="1" defaultvalue="30"/>
<!-- Vne, i.e. maximum airspeed the airframe can handle - used by autopilot, actuator compensation. as well as possibly by INS for plausibility checks -->

2
shared/uavobjectdefinition/vtolpathfollowersettings.xml
View File

@ -9,7 +9,7 @@
<field name="VerticalPosPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="0.4,0.02,1"/>
<field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1,0.01,0,1"/>
<field name="VelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="2"/>
<field name="ThrottleControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="ThrustControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="VelocitySource" units="" type="enum" elements="1" options="EKF,NEDVEL,GPSPOS" defaultvalue="EKF"/>
<field name="PositionSource" units="" type="enum" elements="1" options="EKF,GPSPOS" defaultvalue="EKF"/>
<field name="MaxRollPitch" units="deg" type="float" elements="1" defaultvalue="20"/>