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

Merge branch 'next' of git://git.openpilot.org/OpenPilot into next

Browse Source
This commit is contained in:
Alessio Morale 2014-03-09 14:38:17 +01:00
commit e407eb8126
69 changed files with 2360 additions and 1702 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;

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

@ -129,6 +129,9 @@ int32_t ActuatorInitialize()
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
ActuatorDesiredConnectQueue(queue);
// Register AccessoryDesired (Secondary input to this module)
AccessoryDesiredInitialize();
// Primary output of this module
ActuatorCommandInitialize();
@ -166,6 +169,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 +226,41 @@ 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);
}
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
// safety settings
if (!armed) {
throttleDesired = 0;
}
if (throttleDesired <= 0.00f || !armed) {
// force set all other controls to zero if throttle is cut (previously set in Stabilization)
if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Roll = 0;
}
if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Pitch = 0;
}
if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Yaw = 0;
}
}
#ifdef DIAG_MIXERSTATUS
MixerStatusGet(&mixerStatus);
@ -238,18 +279,18 @@ 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 activeThrottle = (throttleDesired < 0.00f || throttleDesired > 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 +303,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:
@ -295,7 +335,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
continue;
}
if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds);
} else {
status[ct] = -1;
@ -317,6 +357,16 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
}
}
// Reversable Motors are like Motors but go to neutral instead of minimum
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
// If not armed or motor is inactive - no "spinwhilearmed" for this engine type
if (!armed || !activeThrottle) {
filterAccumulator[ct] = 0;
lastResult[ct] = 0;
status[ct] = 0; // force neutral throttle
}
}
// If an accessory channel is selected for direct bypass mode
// In this configuration the accessory channel is scaled and mapped
// directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
@ -419,6 +469,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
// note: no feedforward for reversable motors yet for safety reasons
if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
if (result < 0.0f) { // idle throttle
result = 0.0f;
@ -537,7 +588,7 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
Channel[n] = actuatorSettings->ChannelMin[n];
} else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
} else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
Channel[n] = actuatorSettings->ChannelNeutral[n];
} else {
Channel[n] = 0;

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

@ -116,7 +116,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;
@ -129,7 +129,7 @@ static void altitudeHoldTask(void)
default:
pid_zero(&pid0);
pid_zero(&pid1);
StabilizationDesiredThrottleGet(&startThrottle);
StabilizationDesiredThrustGet(&startThrust);
PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
@ -162,30 +162,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;
}

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

@ -49,7 +49,6 @@
#include "attitudestate.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionstate.h"
#include "manualcontrol.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "airspeedstate.h"
@ -182,52 +181,50 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
uint8_t result;
// Check the combinations of flightmode and pathdesired mode
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
break;
default:
} else {
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
@ -235,8 +232,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
courseIntegral = 0;
speedIntegral = 0;
powerIntegral = 0;
break;
}
PathStatusSet(&pathStatus);
}
@ -357,10 +352,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 +415,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 +469,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 +486,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 +496,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 +511,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

133
flight/modules/ManualControl/altitudehandler.c Normal file
View File

@ -0,0 +1,133 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file altitudehandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
#include <manualcontrolcommand.h>
#include <stabilizationbank.h>
#include <altitudeholddesired.h>
#include <altitudeholdsettings.h>
#include <positionstate.h>
#if defined(REVOLUTION)
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control deprecated flight modes controlled by AltitudeHold module
* @input: ManualControlCommand
* @output: AltitudeHoldDesired
*/
void altitudeHandler(bool newinit)
{
const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
if (newinit) {
StabilizationBankInitialize();
AltitudeHoldDesiredInitialize();
AltitudeHoldSettingsInitialize();
PositionStateInitialize();
}
// 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;
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
FlightStatusFlightModeGet(&flightMode);
AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
AltitudeHoldSettingsThrustExpGet(&thrustExp);
AltitudeHoldSettingsThrustRateGet(&thrustRate);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
PositionStateData posState;
PositionStateGet(&posState);
altitudeHoldDesiredData.Roll = cmd.Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd.Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
if (newinit) {
newaltitude = true;
}
uint8_t cutOff;
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.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 = -((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.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) {
altitudeHoldDesiredData.SetPoint = posState.Down;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
newaltitude = false;
}
AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
}
#else /* if defined(REVOLUTION) */
void altitudeHandler(__attribute__((unused)) bool newinit)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called
}
#endif // REVOLUTION
/**
* @}
* @}
*/

326
flight/modules/ManualControl/armhandler.c Normal file
View File

@ -0,0 +1,326 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file armhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
#include <pios_struct_helper.h>
#include <sanitycheck.h>
#include <manualcontrolcommand.h>
#include <accessorydesired.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
// Private constants
#define ARMED_THRESHOLD 0.50f
// Private types
typedef enum {
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// Private variables
// Private functions
static void setArmedIfChanged(uint8_t val);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool okToArm(void);
static bool forcedDisArm(void);
/**
* @brief Handler to interprete Command inputs in regards to arming/disarming
* @input: ManualControlCommand, AccessoryDesired
* @output: FlightStatus.Arming
*/
void armHandler(bool newinit)
{
static ArmState_t armState;
if (newinit) {
AccessoryDesiredInitialize();
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
armState = ARM_STATE_DISARMED;
}
portTickType sysTime = xTaskGetTickCount();
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
AccessoryDesiredData acc;
bool lowThrottle = cmd.Throttle < 0;
bool armSwitch = false;
switch (settings.Arming) {
case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
AccessoryDesiredInstGet(0, &acc);
armSwitch = true;
break;
case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
AccessoryDesiredInstGet(1, &acc);
armSwitch = true;
break;
case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
AccessoryDesiredInstGet(2, &acc);
armSwitch = true;
break;
default:
break;
}
// immediate disarm on switch
if (armSwitch && acc.AccessoryVal <= -ARMED_THRESHOLD) {
lowThrottle = true;
}
if (forcedDisArm()) {
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
return;
}
if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
return;
}
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch (armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// The rest of these cases throttle is low
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;
}
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
switch (settings.Arming) {
case FLIGHTMODESETTINGS_ARMING_ROLLLEFT:
armingInputLevel = 1.0f * cmd.Roll;
break;
case FLIGHTMODESETTINGS_ARMING_ROLLRIGHT:
armingInputLevel = -1.0f * cmd.Roll;
break;
case FLIGHTMODESETTINGS_ARMING_PITCHFORWARD:
armingInputLevel = 1.0f * cmd.Pitch;
break;
case FLIGHTMODESETTINGS_ARMING_PITCHAFT:
armingInputLevel = -1.0f * cmd.Pitch;
break;
case FLIGHTMODESETTINGS_ARMING_YAWLEFT:
armingInputLevel = 1.0f * cmd.Yaw;
break;
case FLIGHTMODESETTINGS_ARMING_YAWRIGHT:
armingInputLevel = -1.0f * cmd.Yaw;
break;
case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
armingInputLevel = -1.0f * acc.AccessoryVal;
break;
}
bool manualArm = false;
bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD) {
manualArm = true;
} else if (armingInputLevel >= +ARMED_THRESHOLD) {
manualDisarm = true;
}
switch (armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = sysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmingSequenceTime)) {
armState = ARM_STATE_ARMED;
} else if (!manualArm) {
armState = ARM_STATE_DISARMED;
}
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = sysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings.ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmedTimeout)) {
armState = ARM_STATE_DISARMED;
}
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = sysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
// arming switch disarms immediately,
if (manualDisarm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.DisarmingSequenceTime)) {
armState = ARM_STATE_DISARMED;
} else if (!manualDisarm) {
armState = ARM_STATE_ARMED;
}
break;
} // End Switch
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
{
return (end_time - start_time) * portTICK_RATE_MS;
}
/**
* @brief Determine if the aircraft is safe to arm
* @returns True if safe to arm, false otherwise
*/
static bool okToArm(void)
{
// update checks
configuration_check();
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue;
}
return false;
}
}
uint8_t flightMode;
FlightStatusFlightModeGet(&flightMode);
switch (flightMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
return true;
break;
default:
return false;
break;
}
}
/**
* @brief Determine if the aircraft is forced to disarm by an explicit alarm
* @returns True if safe to arm, false otherwise
*/
static bool forcedDisArm(void)
{
// read alarms
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
if (alarms.Receiver == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
return false;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
* @}
* @}
*/

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

@ -6,7 +6,7 @@
* @{
*
* @file manualcontrol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief ManualControl module. Handles safety R/C link and flight mode.
*
* @see The GNU Public License (GPL) Version 3
@ -30,30 +30,57 @@
#ifndef MANUALCONTROL_H
#define MANUALCONTROL_H
#include "manualcontrolcommand.h"
#include <openpilot.h>
#include <flightstatus.h>
typedef enum { FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4 } flightmode_path;
typedef void (*handlerFunc)(bool newinit);
#define PARSE_FLIGHT_MODE(x) \
( \
(x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \
(x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \
FLIGHTMODE_UNDEFINED \
)
typedef struct controlHandlerStruct {
FlightStatusControlChainData controlChain;
handlerFunc handler;
} controlHandler;
int32_t ManualControlInitialize();
/**
* @brief Handler to interprete Command inputs in regards to arming/disarming (called in all flight modes)
* @input: ManualControlCommand, AccessoryDesired
* @output: FlightStatus.Arming
*/
void armHandler(bool newinit);
/**
* @brief Handler to control Manual flightmode - input directly steers actuators
* @input: ManualControlCommand
* @output: ActuatorDesired
*/
void manualHandler(bool newinit);
/**
* @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
* @input: ManualControlCommand
* @output: StabilizationDesired
*/
void stabilizedHandler(bool newinit);
/**
* @brief Handler to control deprecated flight modes controlled by AltitudeHold module
* @input: ManualControlCommand
* @output: AltitudeHoldDesired
*/
void altitudeHandler(bool newinit);
/**
* @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
* @output: PathDesired
*/
void pathFollowerHandler(bool newinit);
/**
* @brief Handler to control Navigated flightmodes. FlightControl is governed by PathFollower, controlled indirectly via PathPlanner
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands to affect navigation
* @output: NONE
*/
void pathPlannerHandler(bool newinit);
/*
* These are assumptions we make in the flight code about the order of settings and their consistency between
@ -61,54 +88,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 \
( \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM))
#endif // MANUALCONTROL_H

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

@ -11,7 +11,7 @@
* AttitudeDesired object (stabilized mode)
*
* @file manualcontrol.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief ManualControl module. Handles safety R/C link and flight mode.
*
* @see The GNU Public License (GPL) Version 3
@ -33,29 +33,16 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "accessorydesired.h"
#include "actuatordesired.h"
#include "altitudeholddesired.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
#include "sanitycheck.h"
#include "manualcontrol.h"
#include "manualcontrolsettings.h"
#include "manualcontrolcommand.h"
#include "positionstate.h"
#include "pathdesired.h"
#include "stabilizationbank.h"
#include "stabilizationdesired.h"
#include "receiveractivity.h"
#include "systemsettings.h"
#include <altitudeholdsettings.h>
#include <taskinfo.h>
#include "inc/manualcontrol.h"
#include <sanitycheck.h>
#include <manualcontrolsettings.h>
#include <manualcontrolcommand.h>
#include <flightmodesettings.h>
#include <flightstatus.h>
#include <systemsettings.h>
#include <stabilizationdesired.h>
#include <callbackinfo.h>
#if defined(PIOS_INCLUDE_USB_RCTX)
#include "pios_usb_rctx.h"
#endif /* PIOS_INCLUDE_USB_RCTX */
// Private constants
#if defined(PIOS_MANUAL_STACK_SIZE)
@ -64,79 +51,98 @@
#define STACK_SIZE_BYTES 1152
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1f
#define ARMED_THRESHOLD 0.50f
// safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 250
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
// defined handlers
static controlHandler handler_MANUAL = {
.controlChain = {
.Stabilization = false,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &manualHandler,
};
static controlHandler handler_STABILIZED = {
.controlChain = {
.Stabilization = true,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &stabilizedHandler,
};
// TODO: move the altitude handling into stabi
static controlHandler handler_ALTITUDE = {
.controlChain = {
.Stabilization = true,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &altitudeHandler,
};
static controlHandler handler_AUTOTUNE = {
.controlChain = {
.Stabilization = false,
.PathFollower = false,
.PathPlanner = false,
},
.handler = NULL,
};
static controlHandler handler_PATHFOLLOWER = {
.controlChain = {
.Stabilization = true,
.PathFollower = true,
.PathPlanner = false,
},
.handler = &pathFollowerHandler,
};
static controlHandler handler_PATHPLANNER = {
.controlChain = {
.Stabilization = true,
.PathFollower = true,
.PathPlanner = true,
},
.handler = &pathPlannerHandler,
};
// Private types
typedef enum {
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// Private variables
static xTaskHandle taskHandle;
static ArmState_t armState;
static portTickType lastSysTime;
#ifdef USE_INPUT_LPF
static portTickType lastSysTimeLPF;
static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
#endif
static DelayedCallbackInfo *callbackHandle;
// Private functions
static void updateActuatorDesired(ManualControlCommandData *cmd);
static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *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 setArmedIfChanged(uint8_t val);
static void configurationUpdatedCb(UAVObjEvent *ev);
static void commandUpdatedCb(UAVObjEvent *ev);
static void manualControlTask(void *parameters);
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool okToArm(void);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
static void applyDeadband(float *value, float deadband);
static void manualControlTask(void);
#ifdef USE_INPUT_LPF
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT);
#endif
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
struct rcvr_activity_fsm {
ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count;
};
static struct rcvr_activity_fsm activity_fsm;
static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode && assumptions_channelcount)
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode)
/**
* Module starting
*/
int32_t ManualControlStart()
{
// Run this initially to make sure the configuration is checked
configuration_check();
// Whenever the configuration changes, make sure it is safe to fly
SystemSettingsConnectCallback(configurationUpdatedCb);
ManualControlSettingsConnectCallback(configurationUpdatedCb);
ManualControlCommandConnectCallback(commandUpdatedCb);
// clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Make sure unarmed on power up
armHandler(true);
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
#endif
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
return 0;
}
@ -147,16 +153,15 @@ int32_t ManualControlStart()
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
if (!assumptions) {
return -1;
}
PIOS_STATIC_ASSERT(assumptions);
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
FlightModeSettingsInitialize();
SystemSettingsInitialize();
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
return 0;
}
@ -165,1103 +170,75 @@ MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
/**
* Module task
*/
static void manualControlTask(__attribute__((unused)) void *parameters)
static void manualControlTask(void)
{
ManualControlSettingsData settings;
// Process Arming
armHandler(false);
// Process flight mode
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
ManualControlCommandData cmd;
FlightStatusData flightStatus;
float flightMode = 0;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
// Run this initially to make sure the configuration is checked
configuration_check();
// Whenever the configuration changes, make sure it is safe to fly
SystemSettingsConnectCallback(configurationUpdatedCb);
ManualControlSettingsConnectCallback(configurationUpdatedCb);
// Whenever the configuration changes, make sure it is safe to fly
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
/* Initialize the RcvrActivty FSM */
portTickType lastActivityTime = xTaskGetTickCount();
resetRcvrActivity(&activity_fsm);
FlightModeSettingsData modeSettings;
FlightModeSettingsGet(&modeSettings);
// Main task loop
lastSysTime = xTaskGetTickCount();
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 };
while (1) {
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
#endif
// Read settings
ManualControlSettingsGet(&settings);
/* Update channel activity monitor */
if (flightStatus.Armed == ARM_STATE_DISARMED) {
if (updateRcvrActivity(&activity_fsm)) {
/* Reset the aging timer because activity was detected */
lastActivityTime = lastSysTime;
}
}
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
resetRcvrActivity(&activity_fsm);
lastActivityTime = lastSysTime;
}
if (ManualControlCommandReadOnly()) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
ManualControlCommandGetMetadata(&metadata);
UAVObjSetAccess(&metadata, ACCESS_READWRITE);
ManualControlCommandSetMetadata(&metadata);
}
}
if (!ManualControlCommandReadOnly()) {
bool valid_input_detected = true;
// Read channel values in us
for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
extern uint32_t pios_rcvr_group_map[];
if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID;
} else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[
cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]],
cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]);
}
// If a channel has timed out this is not valid data and we shouldn't update anything
// until we decide to go to failsafe
if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
valid_input_detected = false;
} else {
scaledChannel[n] = scaleChannel(cmd.Channel[n],
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n],
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n],
cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]);
}
}
// Check settings, if error raise alarm
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID
||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER
|| 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 ||
// 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
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
// Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config)
// immediately disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
continue;
}
// decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin.Throttle,
settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin.Roll,
settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.Yaw,
settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin.Pitch,
settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
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) {
FlightStatusGet(&flightStatus);
cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeBehavior - 1;
flightStatus.FlightMode = settings.FlightModePosition[settings.FailsafeBehavior - 1];
FlightStatusSet(&flightStatus);
}
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
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;
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;
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
} else if (valid_input_detected) {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
// Apply deadband for Roll/Pitch/Yaw stick inputs
if (settings.Deadband > 0.0f) {
applyDeadband(&cmd.Roll, settings.Deadband);
applyDeadband(&cmd.Pitch, settings.Deadband);
applyDeadband(&cmd.Yaw, settings.Deadband);
}
#ifdef USE_INPUT_LPF
// Apply Low Pass Filter to input channels, time delta between calls in ms
portTickType thisSysTime = xTaskGetTickCount();
float dT = (thisSysTime > lastSysTimeLPF) ?
(float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
(float)UPDATE_PERIOD_MS;
lastSysTimeLPF = thisSysTime;
applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
#endif // USE_INPUT_LPF
if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) {
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
}
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY0) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
armSwitch = -1;
}
}
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 1
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY1) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
armSwitch = -1;
}
}
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 2
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY2) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
armSwitch = -1;
}
}
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
processFlightMode(&settings, flightMode, &cmd);
}
// Process arming outside conditional so system will disarm when disconnected
processArm(&cmd, &settings, armSwitch);
// Update cmd object
ManualControlCommandSet(&cmd);
#if defined(PIOS_INCLUDE_USB_RCTX)
if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel,
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll),
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll),
NELEMENTS(cmd.Channel));
}
#endif /* PIOS_INCLUDE_USB_RCTX */
} else {
ManualControlCommandGet(&cmd); /* Under GCS control */
}
FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects
static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break;
case FLIGHTMODE_MANUAL:
updateActuatorDesired(&cmd);
break;
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
break;
case FLIGHTMODE_TUNING:
// Tuning takes settings directly from manualcontrolcommand. No need to
// call anything else. This just avoids errors.
break;
case FLIGHTMODE_GUIDANCE:
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_POI:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
// No need to call anything. This just avoids errors.
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
}
break;
}
lastFlightMode = flightStatus.FlightMode;
}
}
static void resetRcvrActivity(struct rcvr_activity_fsm *fsm)
{
ReceiverActivityData data;
bool updated = false;
/* Clear all channel activity flags */
ReceiverActivityGet(&data);
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
data.ActiveChannel = 255;
updated = true;
}
if (updated) {
ReceiverActivitySet(&data);
uint8_t position = cmd.FlightModeSwitchPosition;
uint8_t newMode = flightStatus.FlightMode;
if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
newMode = modeSettings.FlightModePosition[position];
}
/* Reset the FSM state */
fsm->group = 0;
fsm->sample_count = 0;
}
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
{
for (uint8_t channel = 1; channel <= max_channels; channel++) {
// Subtract 1 because channels are 1 indexed
samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
}
}
static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm)
{
bool activity_updated = false;
/* Compare the current value to the previous sampled value */
for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
uint16_t delta;
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
if (curr > prev) {
delta = curr - prev;
} else {
delta = prev - curr;
}
if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
/* Mark this channel as active */
ReceiverActivityActiveGroupOptions group;
/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
switch (fsm->group) {
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
break;
default:
PIOS_Assert(0);
break;
}
ReceiverActivityActiveGroupSet((uint8_t *)&group);
ReceiverActivityActiveChannelSet(&channel);
activity_updated = true;
}
}
return activity_updated;
}
static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm)
{
bool activity_updated = false;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* We're out of range, reset things */
resetRcvrActivity(fsm);
}
extern uint32_t pios_rcvr_group_map[];
if (!pios_rcvr_group_map[fsm->group]) {
/* Unbound group, skip it */
goto group_completed;
}
if (fsm->sample_count == 0) {
/* Take a sample of each channel in this group */
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++;
return false;
}
/* Compare with previous sample */
activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
group_completed:
/* Reset the sample counter */
fsm->sample_count = 0;
/* Find the next active group, but limit search so we can't loop forever here */
for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
/* Move to the next group */
fsm->group++;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* Wrap back to the first group */
fsm->group = 0;
}
if (pios_rcvr_group_map[fsm->group]) {
/*
* Found an active group, take a sample here to avoid an
* extra 20ms delay in the main thread so we can speed up
* this algorithm.
*/
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++;
break;
}
}
return activity_updated;
}
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;
ActuatorDesiredSet(&actuator);
}
static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings)
{
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
uint8_t *stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = cast_struct_to_array(settings->Stabilization3Settings, settings->Stabilization3Settings.Roll);
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode.Roll = stab_settings[0];
stabilization.StabilizationMode.Pitch = stab_settings[1];
// Other axes (yaw) cannot be Rattitude, so use Rate
// Should really do this for Attitude mode as well?
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
} else {
stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
0; // this is an invalid mode
}
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
}
#if defined(REVOLUTION)
// TODO: Need compile flag to exclude this from copter control
/**
* @brief Update the position desired to current location when
* enabled and allow the waypoint to be moved by transmitter
*/
static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed, bool home)
{
/*
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
*/
if (home && changed) {
// Simple Return To Base mode - keep altitude the same, fly to home position
PositionStateData positionState;
PositionStateGet(&positionState);
ManualControlSettingsData settings;
ManualControlSettingsGet(&settings);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start.North = 0;
pathDesired.Start.East = 0;
pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End.North = 0;
pathDesired.End.East = 0;
pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else if (changed) {
// After not being in this mode for a while init at current height
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
} else {
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
*/
}
}
static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed)
{
/*
static portTickType lastSysTime;
portTickType thisSysTime;
float dT;
thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime;
*/
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
if (changed) {
// After not being in this mode for a while init at current height
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
}
pathDesired.End.Down = positionState.Down + 5;
PathDesiredSet(&pathDesired);
}
/**
* @brief Update the altitude desired to current altitude when
* enabled and enable altitude mode for stabilization
* @todo: Need compile flag to exclude this from copter control
*/
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
{
const float DEADBAND = 0.20f;
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;
static uint8_t flightMode;
static bool newaltitude = true;
FlightStatusFlightModeGet(&flightMode);
AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
PositionStateData posState;
PositionStateGet(&posState);
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
if (changed) {
newaltitude = true;
}
uint8_t cutOff;
AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff);
if (cutOff && cmd->Throttle < 0) {
// Cut throttle if desired
altitudeHoldDesiredData.SetPoint = cmd->Throttle;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > 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.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);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (newaltitude == true) {
altitudeHoldDesiredData.SetPoint = posState.Down;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
newaltitude = false;
}
AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
}
#else /* if defined(REVOLUTION) */
// TODO: These functions should never be accessible on CC. Any configuration that
// could allow them to be called should already throw an error to prevent this happening
// in flight
static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd,
__attribute__((unused)) bool changed,
__attribute__((unused)) bool home)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd,
__attribute__((unused)) bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData *cmd,
__attribute__((unused)) bool changed)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
#endif /* if defined(REVOLUTION) */
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{
float valueScaled;
// Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
if (max != neutral) {
valueScaled = (float)(value - neutral) / (float)(max - neutral);
} else {
valueScaled = 0;
}
} else {
if (min != neutral) {
valueScaled = (float)(value - neutral) / (float)(neutral - min);
} else {
valueScaled = 0;
}
}
// Bound
if (valueScaled > 1.0f) {
valueScaled = 1.0f;
} else if (valueScaled < -1.0f) {
valueScaled = -1.0f;
}
return valueScaled;
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
{
return (end_time - start_time) * portTICK_RATE_MS;
}
/**
* @brief Determine if the aircraft is safe to arm
* @returns True if safe to arm, false otherwise
*/
static bool okToArm(void)
{
// update checks
configuration_check();
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue;
}
return false;
}
}
uint8_t flightMode;
FlightStatusFlightModeGet(&flightMode);
switch (flightMode) {
// Depending on the mode update the Stabilization or Actuator objects
controlHandler *handler = &handler_MANUAL;
switch (newMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
handler = &handler_MANUAL;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
return true;
default:
return false;
}
}
/**
* @brief Determine if the aircraft is forced to disarm by an explicit alarm
* @returns True if safe to arm, false otherwise
*/
static bool forcedDisArm(void)
{
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
if (alarms.Alarm.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
return false;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Process the inputs and determine whether to arm or not
* @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)
{
bool lowThrottle = cmd->Throttle < 0;
/**
* 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:
if (armSwitch < 0) {
lowThrottle = true;
}
handler = &handler_STABILIZED;
break;
default:
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_POI:
handler = &handler_PATHFOLLOWER;
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
handler = &handler_PATHPLANNER;
break;
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
handler = &handler_ALTITUDE;
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
handler = &handler_AUTOTUNE;
break;
// There is no default, so if a flightmode is forgotten the compiler can throw a warning!
}
if (forcedDisArm()) {
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
return;
}
bool newinit = false;
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
lowThrottle = true;
}
// FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid)
static bool firstRun = true;
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch (armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
switch (settings->Arming) {
case MANUALCONTROLSETTINGS_ARMING_ROLLLEFT:
armingInputLevel = 1.0f * cmd->Roll;
break;
case MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT:
armingInputLevel = -1.0f * cmd->Roll;
break;
case MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD:
armingInputLevel = 1.0f * cmd->Pitch;
break;
case MANUALCONTROLSETTINGS_ARMING_PITCHAFT:
armingInputLevel = -1.0f * cmd->Pitch;
break;
case MANUALCONTROLSETTINGS_ARMING_YAWLEFT:
armingInputLevel = 1.0f * cmd->Yaw;
break;
case MANUALCONTROLSETTINGS_ARMING_YAWRIGHT:
armingInputLevel = -1.0f * cmd->Yaw;
break;
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2:
armingInputLevel = -1.0f * (float)armSwitch;
break;
}
bool manualArm = false;
bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD) {
manualArm = true;
} else if (armingInputLevel >= +ARMED_THRESHOLD) {
manualDisarm = true;
}
switch (armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmingSequenceTime)) {
armState = ARM_STATE_ARMED;
} else if (!manualArm) {
armState = ARM_STATE_DISARMED;
}
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) {
armState = ARM_STATE_DISARMED;
}
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->DisarmingSequenceTime)) {
armState = ARM_STATE_DISARMED;
} else if (!manualDisarm) {
armState = ARM_STATE_ARMED;
}
break;
} // End Switch
}
}
/**
* @brief Determine which of N positions the flight mode switch is in and set flight mode accordingly
* @param[out] cmd Pointer to the command structure to set the flight mode in
* @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)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Convert flightMode value into the switch position in the range [0..N-1]
uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9;
if (pos >= settings->FlightModeNumber) {
pos = settings->FlightModeNumber - 1;
}
cmd->FlightModeSwitchPosition = pos;
uint8_t newMode = settings->FlightModePosition[pos];
if (flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
if (flightStatus.FlightMode != newMode || firstRun) {
firstRun = false;
flightStatus.ControlChain = handler->controlChain;
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
newinit = true;
}
if (handler->handler) {
handler->handler(newinit);
}
}
/**
* @brief Determine if the manual input value is within acceptable limits
* @returns return TRUE if so, otherwise return FALSE
*/
bool validInputRange(int16_t min, int16_t max, uint16_t value)
{
if (min > max) {
int16_t tmp = min;
min = max;
max = tmp;
}
return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET;
}
/**
* @brief Apply deadband to Roll/Pitch/Yaw channels
*/
static void applyDeadband(float *value, float deadband)
{
if (fabsf(*value) < deadband) {
*value = 0.0f;
} else if (*value > 0.0f) {
*value -= deadband;
} else {
*value += deadband;
}
}
#ifdef USE_INPUT_LPF
/**
* @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel
*/
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{
if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) {
float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel];
}
}
#endif // USE_INPUT_LPF
/**
* Called whenever a critical configuration component changes
*/
@ -1270,6 +247,14 @@ static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
configuration_check();
}
/**
* Called whenever a critical configuration component changes
*/
static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
}
/**
* @}
* @}

71
flight/modules/ManualControl/manualhandler.c Normal file
View File

@ -0,0 +1,71 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file manualhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
#include <manualcontrolcommand.h>
#include <actuatordesired.h>
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control Manual flightmode - input directly steers actuators
* @input: ManualControlCommand
* @output: ActuatorDesired
*/
void manualHandler(bool newinit)
{
if (newinit) {
ActuatorDesiredInitialize();
}
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator);
actuator.Roll = cmd.Roll;
actuator.Pitch = cmd.Pitch;
actuator.Yaw = cmd.Yaw;
actuator.Thrust = cmd.Thrust;
ActuatorDesiredSet(&actuator);
}
/**
* @}
* @}
*/

131
flight/modules/ManualControl/pathfollowerhandler.c Normal file
View File

@ -0,0 +1,131 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file pathfollowerhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
#include <pathdesired.h>
#include <manualcontrolcommand.h>
#include <flightstatus.h>
#include <positionstate.h>
#include <flightmodesettings.h>
#if defined(REVOLUTION)
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
* @output: PathDesired
*/
void pathFollowerHandler(bool newinit)
{
if (newinit) {
PathDesiredInitialize();
PositionStateInitialize();
}
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (newinit) {
// After not being in this mode for a while init at current height
PositionStateData positionState;
PositionStateGet(&positionState);
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
// Simple Return To Base mode - keep altitude the same, fly to home position
pathDesired.Start.North = 0;
pathDesired.Start.East = 0;
pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End.North = 0;
pathDesired.End.East = 0;
pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
break;
default:
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
} else {
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
*/
break;
}
PathDesiredSet(&pathDesired);
}
// special handling of autoland - behaves like positon hold but with slow altitude decrease
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End.Down = positionState.Down + 5;
PathDesiredSet(&pathDesired);
}
}
#else /* if defined(REVOLUTION) */
void pathFollowerHandler(__attribute__((unused)) bool newinit)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called
}
#endif // REVOLUTION
/**
* @}
* @}
*/

64
flight/modules/ManualControl/pathplannerhandler.c Normal file
View File

@ -0,0 +1,64 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file pathplannerhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control Navigated flightmodes. FlightControl is governed by PathFollower, controlled indirectly via PathPlanner
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands to affect navigation
* @output: NONE
*/
void pathPlannerHandler(__attribute__((unused)) bool newinit)
{
/**
*
* TODO: In fully autonomous mode, commands to the navigation facility
* can be encoded through standard input cmd channels, as the pathplanner
* pathfollower generally ignores them
*
* this should be provided by a separate handler invoked here, as the
* handler for pathFollower should likely invoke them as well!!!
* (inputs also ignored)
*
*/
}
/**
* @}
* @}
*/

143
flight/modules/ManualControl/stabilizedhandler.c Normal file
View File

@ -0,0 +1,143 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControl
* @brief Interpretes the control input in ManualControlCommand
* @{
*
* @file stabilizedhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/manualcontrol.h"
#include <pios_struct_helper.h>
#include <manualcontrolcommand.h>
#include <stabilizationdesired.h>
#include <flightmodesettings.h>
#include <stabilizationbank.h>
// Private constants
// Private types
// Private functions
/**
* @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
* @input: ManualControlCommand
* @output: StabilizationDesired
*/
void stabilizedHandler(bool newinit)
{
if (newinit) {
StabilizationDesiredInitialize();
StabilizationBankInitialize();
}
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
uint8_t *stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = cast_struct_to_array(settings.Stabilization2Settings, settings.Stabilization2Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization3Settings.Roll);
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll);
return;
}
stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Roll * stabSettings.RollMax :
0; // this is an invalid mode
stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode.Roll = stab_settings[0];
stabilization.StabilizationMode.Pitch = stab_settings[1];
// Other axes (yaw) cannot be Rattitude, so use Rate
// Should really do this for Attitude mode as well?
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
} else {
stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
0; // this is an invalid mode
}
stabilization.Thrust = cmd.Thrust;
StabilizationDesiredSet(&stabilization);
}
/**
* @}
* @}
*/

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

@ -42,7 +42,7 @@
#include "velocitystate.h"
#include "waypoint.h"
#include "waypointactive.h"
#include "manualcontrolsettings.h"
#include "flightmodesettings.h"
#include <pios_struct_helper.h>
#include "paths.h"
@ -139,7 +139,7 @@ static void pathPlannerTask()
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
pathplanner_active = false;
if (!validPathPlan) {
// unverified path plans are only a warning while we are not in pathplanner mode
@ -171,8 +171,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;

673
flight/modules/Receiver/receiver.c Normal file
View File

@ -0,0 +1,673 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ReceiverModule Manual Control Module
* @brief Provide manual control or allow it alter flight mode.
* @{
*
* Reads in the ManualControlCommand from receiver then
* pass it to ManualControl
*
* @file receiver.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Receiver module. Handles safety R/C link and flight mode.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <accessorydesired.h>
#include <manualcontrolsettings.h>
#include <manualcontrolcommand.h>
#include <receiveractivity.h>
#include <flightstatus.h>
#include <flighttelemetrystats.h>
#include <flightmodesettings.h>
#include <systemsettings.h>
#include <taskinfo.h>
#if defined(PIOS_INCLUDE_USB_RCTX)
#include "pios_usb_rctx.h"
#endif /* PIOS_INCLUDE_USB_RCTX */
// Private constants
#if defined(PIOS_RECEIVER_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_RECEIVER_STACK_SIZE
#else
#define STACK_SIZE_BYTES 1152
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1f
#define ARMED_THRESHOLD 0.50f
// safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 250
// Private types
// Private variables
static xTaskHandle taskHandle;
static portTickType lastSysTime;
#ifdef USE_INPUT_LPF
static portTickType lastSysTimeLPF;
static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
#endif
// Private functions
static void receiverTask(void *parameters);
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
static void applyDeadband(float *value, float deadband);
#ifdef USE_INPUT_LPF
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT);
#endif
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
struct rcvr_activity_fsm {
ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count;
};
static struct rcvr_activity_fsm activity_fsm;
static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
#define assumptions \
( \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM))
/**
* Module starting
*/
int32_t ReceiverStart()
{
// Start main task
xTaskCreate(receiverTask, (signed char *)"Receiver", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RECEIVER, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
#endif
return 0;
}
/**
* Module initialization
*/
int32_t ReceiverInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
PIOS_STATIC_ASSERT(assumptions);
ManualControlCommandInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
return 0;
}
MODULE_INITCALL(ReceiverInitialize, ReceiverStart);
/**
* Module task
*/
static void receiverTask(__attribute__((unused)) void *parameters)
{
ManualControlSettingsData settings;
ManualControlCommandData cmd;
FlightStatusData flightStatus;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
// Whenever the configuration changes, make sure it is safe to fly
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
/* Initialize the RcvrActivty FSM */
portTickType lastActivityTime = xTaskGetTickCount();
resetRcvrActivity(&activity_fsm);
// Main task loop
lastSysTime = xTaskGetTickCount();
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 };
SystemSettingsThrustControlOptions thrustType;
while (1) {
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
#endif
// Read settings
ManualControlSettingsGet(&settings);
SystemSettingsThrustControlGet(&thrustType);
/* Update channel activity monitor */
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
if (updateRcvrActivity(&activity_fsm)) {
/* Reset the aging timer because activity was detected */
lastActivityTime = lastSysTime;
}
}
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
resetRcvrActivity(&activity_fsm);
lastActivityTime = lastSysTime;
}
if (ManualControlCommandReadOnly()) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
ManualControlCommandGetMetadata(&metadata);
UAVObjSetAccess(&metadata, ACCESS_READWRITE);
ManualControlCommandSetMetadata(&metadata);
}
}
bool valid_input_detected = true;
// Read channel values in us
for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
extern uint32_t pios_rcvr_group_map[];
if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID;
} else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[
cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]],
cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]);
}
// If a channel has timed out this is not valid data and we shouldn't update anything
// until we decide to go to failsafe
if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
valid_input_detected = false;
} else {
scaledChannel[n] = scaleChannel(cmd.Channel[n],
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n],
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n],
cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]);
}
}
// Check settings, if error raise alarm
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID
||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER
||
// Check collective if required
(thrustType == SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE && (
settings.ChannelGroups.Collective >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_NODRIVER))
||
// Check the FlightModeNumber is valid
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
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
continue;
}
// decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin.Throttle,
settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin.Roll,
settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.Yaw,
settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin.Pitch,
settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
if (settings.ChannelGroups.Collective != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
valid_input_detected &= validInputRange(settings.ChannelMin.Collective,
settings.ChannelMax.Collective, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]);
}
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
valid_input_detected &= validInputRange(settings.ChannelMin.Accessory0,
settings.ChannelMax.Accessory0, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]);
}
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
valid_input_detected &= validInputRange(settings.ChannelMin.Accessory1,
settings.ChannelMax.Accessory1, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]);
}
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
valid_input_detected &= validInputRange(settings.ChannelMin.Accessory2,
settings.ChannelMax.Accessory2, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]);
}
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
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) {
cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition;
}
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = settings.FailsafeChannel.Accessory0;
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 1
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = settings.FailsafeChannel.Accessory1;
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 2
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = settings.FailsafeChannel.Accessory2;
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
} else if (valid_input_detected) {
AlarmsClear(SYSTEMALARMS_ALARM_RECEIVER);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
// Convert flightMode value into the switch position in the range [0..N-1]
cmd.FlightModeSwitchPosition = ((int16_t)(scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] * 256.0f) + 256) * settings.FlightModeNumber >> 9;
if (cmd.FlightModeSwitchPosition >= settings.FlightModeNumber) {
cmd.FlightModeSwitchPosition = settings.FlightModeNumber - 1;
}
// Apply deadband for Roll/Pitch/Yaw stick inputs
if (settings.Deadband > 0.0f) {
applyDeadband(&cmd.Roll, settings.Deadband);
applyDeadband(&cmd.Pitch, settings.Deadband);
applyDeadband(&cmd.Yaw, settings.Deadband);
}
#ifdef USE_INPUT_LPF
// Apply Low Pass Filter to input channels, time delta between calls in ms
portTickType thisSysTime = xTaskGetTickCount();
float dT = (thisSysTime > lastSysTimeLPF) ?
(float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
(float)UPDATE_PERIOD_MS;
lastSysTimeLPF = thisSysTime;
applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
#endif // USE_INPUT_LPF
if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) {
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
if (settings.Deadband > 0.0f) {
applyDeadband(&cmd.Collective, settings.Deadband);
}
#ifdef USE_INPUT_LPF
applyLPF(&cmd.Collective, MANUALCONTROLSETTINGS_RESPONSETIME_COLLECTIVE, &settings, dT);
#endif // USE_INPUT_LPF
}
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) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 1
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 2
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
}
}
}
// Update cmd object
ManualControlCommandSet(&cmd);
#if defined(PIOS_INCLUDE_USB_RCTX)
if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel,
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll),
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll),
NELEMENTS(cmd.Channel));
}
#endif /* PIOS_INCLUDE_USB_RCTX */
}
}
static void resetRcvrActivity(struct rcvr_activity_fsm *fsm)
{
ReceiverActivityData data;
bool updated = false;
/* Clear all channel activity flags */
ReceiverActivityGet(&data);
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
data.ActiveChannel = 255;
updated = true;
}
if (updated) {
ReceiverActivitySet(&data);
}
/* Reset the FSM state */
fsm->group = 0;
fsm->sample_count = 0;
}
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
{
for (uint8_t channel = 1; channel <= max_channels; channel++) {
// Subtract 1 because channels are 1 indexed
samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
}
}
static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm)
{
bool activity_updated = false;
/* Compare the current value to the previous sampled value */
for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
uint16_t delta;
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
if (curr > prev) {
delta = curr - prev;
} else {
delta = prev - curr;
}
if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
/* Mark this channel as active */
ReceiverActivityActiveGroupOptions group;
/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
switch (fsm->group) {
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
break;
default:
PIOS_Assert(0);
break;
}
ReceiverActivityActiveGroupSet((uint8_t *)&group);
ReceiverActivityActiveChannelSet(&channel);
activity_updated = true;
}
}
return activity_updated;
}
static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm)
{
bool activity_updated = false;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* We're out of range, reset things */
resetRcvrActivity(fsm);
}
extern uint32_t pios_rcvr_group_map[];
if (!pios_rcvr_group_map[fsm->group]) {
/* Unbound group, skip it */
goto group_completed;
}
if (fsm->sample_count == 0) {
/* Take a sample of each channel in this group */
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++;
return false;
}
/* Compare with previous sample */
activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
group_completed:
/* Reset the sample counter */
fsm->sample_count = 0;
/* Find the next active group, but limit search so we can't loop forever here */
for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
/* Move to the next group */
fsm->group++;
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* Wrap back to the first group */
fsm->group = 0;
}
if (pios_rcvr_group_map[fsm->group]) {
/*
* Found an active group, take a sample here to avoid an
* extra 20ms delay in the main thread so we can speed up
* this algorithm.
*/
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++;
break;
}
}
return activity_updated;
}
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{
float valueScaled;
// Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
if (max != neutral) {
valueScaled = (float)(value - neutral) / (float)(max - neutral);
} else {
valueScaled = 0;
}
} else {
if (min != neutral) {
valueScaled = (float)(value - neutral) / (float)(neutral - min);
} else {
valueScaled = 0;
}
}
// Bound
if (valueScaled > 1.0f) {
valueScaled = 1.0f;
} else if (valueScaled < -1.0f) {
valueScaled = -1.0f;
}
return valueScaled;
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
{
return (end_time - start_time) * portTICK_RATE_MS;
}
/**
* @brief Determine if the manual input value is within acceptable limits
* @returns return TRUE if so, otherwise return FALSE
*/
bool validInputRange(int16_t min, int16_t max, uint16_t value)
{
if (min > max) {
int16_t tmp = min;
min = max;
max = tmp;
}
return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET;
}
/**
* @brief Apply deadband to Roll/Pitch/Yaw channels
*/
static void applyDeadband(float *value, float deadband)
{
if (fabsf(*value) < deadband) {
*value = 0.0f;
} else if (*value > 0.0f) {
*value -= deadband;
} else {
*value += deadband;
}
}
#ifdef USE_INPUT_LPF
/**
* @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel
*/
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{
if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) {
float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel];
}
}
#endif // USE_INPUT_LPF
/**
* @}
* @}
*/

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

@ -49,7 +49,8 @@
#include "gyrostate.h"
#include "flightstatus.h"
#include "manualcontrolsettings.h"
#include "manualcontrol.h" // Just to get a macro
#include "manualcontrolcommand.h"
#include "flightmodesettings.h"
#include "taskinfo.h"
// Math libraries
@ -75,7 +76,7 @@
#if defined(PIOS_STABILIZATION_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE
#else
#define STACK_SIZE_BYTES 840
#define STACK_SIZE_BYTES 860
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority
@ -103,15 +104,14 @@ uint8_t max_axislock_rate = 0;
float weak_leveling_kp = 0;
uint8_t weak_leveling_max = 0;
bool lowThrottleZeroIntegral;
bool lowThrottleZeroAxis[MAX_AXES];
float vbar_decay = 0.991f;
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,9 +168,13 @@ 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
ManualControlCommandInitialize();
ManualControlSettingsInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
StabilizationSettingsInitialize();
StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
@ -205,6 +209,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
float throttleDesired;
RateDesiredData rateDesired;
AttitudeStateData attitudeState;
GyroStateData gyroStateData;
@ -236,6 +241,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,26 +598,11 @@ 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) {
if (lowThrottleZeroAxis[ROLL]) {
actuatorDesired.Roll = 0.0f;
}
if (lowThrottleZeroAxis[PITCH]) {
actuatorDesired.Pitch = 0.0f;
}
if (lowThrottleZeroAxis[YAW]) {
actuatorDesired.Yaw = 0.0f;
}
}
// 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 +636,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,20 +644,20 @@ 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;
}
}
}
if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuatorDesired);
} else {
// Force all axes to reinitialize when engaged
@ -676,7 +667,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,13 +897,8 @@ 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
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle 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;
// Whether to zero the PID integrals while thrust is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant
@ -933,10 +919,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;

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

@ -55,7 +55,7 @@
#include "hwsettings.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionstate.h"
#include "manualcontrol.h"
#include "manualcontrolcommand.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "gpsvelocitysensor.h"
@ -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.
*/
@ -206,55 +206,53 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
PathDesiredGet(&pathDesired);
// Check the combinations of flightmode and pathdesired mode
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
} else {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
break;
case FLIGHTSTATUS_FLIGHTMODE_POI:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
} else {
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
@ -263,12 +261,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;
break;
thrustOffset = stabDesired.Thrust;
}
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
@ -550,10 +546,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 +649,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 +666,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;

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

@ -34,6 +34,7 @@ ERASE_FLASH ?= NO
MODULES += Attitude
MODULES += Stabilization
MODULES += Actuator
MODULES += Receiver
MODULES += ManualControl
MODULES += FirmwareIAP
MODULES += Telemetry
@ -98,6 +99,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/coptercontrol/firmware/inc/pios_config.h
View File

@ -164,7 +164,6 @@
#else
#define PIOS_SYSTEM_STACK_SIZE 660
#endif
#define PIOS_STABILIZATION_STACK_SIZE 790
#define PIOS_TELEM_STACK_SIZE 540
#define PIOS_EVENTDISPATCHER_STACK_SIZE 160

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

@ -37,6 +37,7 @@ MODULES += AltitudeHold
MODULES += Stabilization
MODULES += VtolPathFollower
MODULES += ManualControl
MODULES += Receiver
MODULES += Actuator
MODULES += GPS
MODULES += TxPID

1
flight/targets/boards/revolution/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/revoproto/firmware/Makefile
View File

@ -35,6 +35,7 @@ MODULES += Altitude/revolution
MODULES += Airspeed
MODULES += AltitudeHold
MODULES += Stabilization
MODULES += Receiver
MODULES += ManualControl
MODULES += Actuator
MODULES += GPS

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

@ -62,6 +62,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

18
ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg
View File

@ -701,7 +701,7 @@
<rect
inkscape:label="#rect4000-8-0-9"
style="fill:#332d2d;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
id="ManualControl"
id="Receiver"
width="13.893178"
height="56.637238"
x="576.71594"
@ -1135,7 +1135,7 @@
</g>
<g
style="display:none"
inkscape:label="ManualControl-OK"
inkscape:label="Receiver-OK"
id="layer36"
inkscape:groupmode="layer">
<rect
@ -1143,14 +1143,14 @@
x="576.71594"
height="56.637238"
width="13.893178"
id="ManualControl-OK"
id="Receiver-OK"
style="fill:#04b629;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
inkscape:label="#rect4000-8-0-9"
transform="translate(-497.66563,-344.28037)" />
</g>
<g
style="display:none"
inkscape:label="ManualControl-Warning"
inkscape:label="Receiver-Warning"
id="layer33"
inkscape:groupmode="layer">
<rect
@ -1158,19 +1158,19 @@
x="576.71594"
height="56.637238"
width="13.893178"
id="ManualControl-Warning"
id="Receiver-Warning"
style="fill:#f1b907;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
inkscape:label="#rect4000-8-0-9"
transform="translate(-497.66563,-344.28037)" />
</g>
<g
style="display:none"
inkscape:label="ManualControl-Error"
inkscape:label="Receiver-Error"
id="layer34"
inkscape:groupmode="layer">
<g
transform="translate(78,0)"
id="ManualControl-Error"
id="Receiver-Error"
style="stroke:#cf0e0e;stroke-opacity:1;display:inline"
inkscape:label="#g3878">
<path
@ -1185,14 +1185,14 @@
</g>
<g
style="display:none"
inkscape:label="ManualControl-Critical"
inkscape:label="Receiver-Critical"
id="layer35"
inkscape:groupmode="layer">
<rect
transform="translate(-497.66563,-344.28037)"
inkscape:label="#rect4000-8-0-9"
style="fill:#cf0e0e;fill-opacity:1;stroke:#ffffff;stroke-width:0.53149605;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline"
id="ManualControl-Critical"
id="Receiver-Critical"
width="13.893178"
height="56.637238"
x="576.71594"

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 235 KiB

After

Width:  |  Height:  |  Size: 235 KiB

2
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp
View File

@ -181,6 +181,8 @@ QString ConfigCustomWidget::updateConfigObjectsFromWidgets()
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_DISABLED);
} else if (q->currentText() == "Motor") {
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR);
} else if (q->currentText() == "ReversableMotor") {
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR);
} else if (q->currentText() == "Servo") {
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
} else if (q->currentText() == "CameraRoll") {

10
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp
View File

@ -321,7 +321,7 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleMotorcycle(QString airframeTyp
// motor
int channel = m_aircraft->gvMotor2ChannelBox->currentIndex() - 1;
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR);
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127);
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, 127);
@ -371,13 +371,13 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleDifferential(QString airframeT
// left motor
int channel = m_aircraft->gvMotor1ChannelBox->currentIndex() - 1;
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR);
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127);
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, 127);
// right motor
channel = m_aircraft->gvMotor2ChannelBox->currentIndex() - 1;
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR);
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, 127);
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -127);
@ -424,11 +424,11 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleCar(QString airframeType)
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -127);
channel = m_aircraft->gvMotor1ChannelBox->currentIndex() - 1;
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR);
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127);
channel = m_aircraft->gvMotor2ChannelBox->currentIndex() - 1;
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO);
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR);
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, 127);
// Output success message

4
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp
View File

@ -42,7 +42,7 @@ VehicleConfig::VehicleConfig(QWidget *parent) : ConfigTaskWidget(parent)
channelNames << QString("Channel%1").arg(i + 1);
}
mixerTypeDescriptions << "Disabled" << "Motor" << "Servo" << "CameraRoll" << "CameraPitch" << "CameraYaw"
mixerTypeDescriptions << "Disabled" << "Motor" << "ReversableMotor" << "Servo" << "CameraRoll" << "CameraPitch" << "CameraYaw"
<< "Accessory0" << "Accessory1" << "Accessory2" << "Accessory3" << "Accessory4" << "Accessory5";
// This is needed because new style tries to compact things as much as possible in grid
@ -224,7 +224,7 @@ void VehicleConfig::resetMotorAndServoMixers(UAVDataObject *mixer)
{
for (int channel = 0; channel < (int)VehicleConfig::CHANNEL_NUMELEM; channel++) {
QString type = getMixerType(mixer, channel);
if ((type == "Disabled") || (type == "Motor") || (type == "Servo")) {
if ((type == "Disabled") || (type == "Motor") || (type == "ReversableMotor") || (type == "Servo")) {
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_DISABLED);
resetMixerVector(mixer, channel);
}

25
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h
View File

@ -121,18 +121,19 @@ public:
/* Enumeration options for field MixerType */
typedef enum {
MIXERTYPE_DISABLED = 0,
MIXERTYPE_MOTOR = 1,
MIXERTYPE_SERVO = 2,
MIXERTYPE_CAMERAROLL = 3,
MIXERTYPE_CAMERAPITCH = 4,
MIXERTYPE_CAMERAYAW = 5,
MIXERTYPE_ACCESSORY0 = 6,
MIXERTYPE_ACCESSORY1 = 7,
MIXERTYPE_ACCESSORY2 = 8,
MIXERTYPE_ACCESSORY3 = 9,
MIXERTYPE_ACCESSORY4 = 10,
MIXERTYPE_ACCESSORY5 = 11
MIXERTYPE_DISABLED = 0,
MIXERTYPE_MOTOR = 1,
MIXERTYPE_REVERSABLEMOTOR = 2,
MIXERTYPE_SERVO = 3,
MIXERTYPE_CAMERAROLL = 4,
MIXERTYPE_CAMERAPITCH = 5,
MIXERTYPE_CAMERAYAW = 6,
MIXERTYPE_ACCESSORY0 = 7,
MIXERTYPE_ACCESSORY1 = 8,
MIXERTYPE_ACCESSORY2 = 9,
MIXERTYPE_ACCESSORY3 = 10,
MIXERTYPE_ACCESSORY4 = 11,
MIXERTYPE_ACCESSORY5 = 12
} MixerTypeElem;
/* Array element names for field MixerVector */

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);
@ -134,26 +135,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()));
@ -367,10 +368,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);
@ -400,6 +403,7 @@ void ConfigInputWidget::wzCancel()
// Load settings back from beginning of wizard
manualSettingsObj->setData(previousManualSettingsData);
flightModeSettingsObj->setData(previousFlightModeSettingsData);
}
void ConfigInputWidget::wzNext()
@ -1220,13 +1224,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);
}
@ -1420,11 +1424,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

@ -24195,7 +24195,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">
@ -24271,7 +24271,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>
@ -24492,7 +24492,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>
@ -24502,7 +24502,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>
@ -24519,7 +24519,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>
@ -24580,7 +24580,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>
@ -24614,7 +24614,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>
@ -26711,7 +26711,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>
@ -26784,7 +26784,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"/>
@ -26807,7 +26807,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>
@ -26881,7 +26881,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>
@ -26906,7 +26906,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>
@ -27457,7 +27457,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>

14
ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.cpp
View File

@ -242,13 +242,14 @@ void FlightLogManager::exportToOPL(QString fileName)
void FlightLogManager::exportToCSV(QString fileName)
{
QFile csvFile(fileName);
if (csvFile.open(QFile::WriteOnly | QFile::Truncate)) {
QTextStream csvStream(&csvFile);
quint32 baseTime = 0;
quint32 currentFlight = 0;
csvStream << "Flight" << '\t' << "Flight Time" << '\t' << "Entry" << '\t' << "Data" << '\n';
foreach (ExtendedDebugLogEntry *entry , m_logEntries) {
if(m_adjustExportedTimestamps && entry->getFlight() != currentFlight) {
foreach(ExtendedDebugLogEntry * entry, m_logEntries) {
if (m_adjustExportedTimestamps && entry->getFlight() != currentFlight) {
currentFlight = entry->getFlight();
baseTime = entry->getFlightTime();
}
@ -263,8 +264,8 @@ void FlightLogManager::exportToCSV(QString fileName)
void FlightLogManager::exportToXML(QString fileName)
{
QFile xmlFile(fileName);
if (xmlFile.open(QFile::WriteOnly | QFile::Truncate)) {
if (xmlFile.open(QFile::WriteOnly | QFile::Truncate)) {
QXmlStreamWriter xmlWriter(&xmlFile);
xmlWriter.setAutoFormatting(true);
xmlWriter.setAutoFormattingIndent(4);
@ -275,8 +276,8 @@ void FlightLogManager::exportToXML(QString fileName)
quint32 baseTime = 0;
quint32 currentFlight = 0;
foreach (ExtendedDebugLogEntry *entry , m_logEntries) {
if(m_adjustExportedTimestamps && entry->getFlight() != currentFlight) {
foreach(ExtendedDebugLogEntry * entry, m_logEntries) {
if (m_adjustExportedTimestamps && entry->getFlight() != currentFlight) {
currentFlight = entry->getFlight();
baseTime = entry->getFlightTime();
}
@ -388,12 +389,13 @@ void ExtendedDebugLogEntry::toXML(QXmlStreamWriter *xmlWriter, quint32 baseTime)
xmlWriter->writeAttribute("type", "uavobject");
m_object->toXML(xmlWriter);
}
xmlWriter->writeEndElement(); //entry
xmlWriter->writeEndElement(); // entry
}
void ExtendedDebugLogEntry::toCSV(QTextStream *csvStream, quint32 baseTime)
{
QString data;
if (getType() == DebugLogEntry::TYPE_TEXT) {
data = QString((const char *)getData().Data);
} else if (getType() == DebugLogEntry::TYPE_UAVOBJECT) {

1
ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h
View File

@ -165,7 +165,6 @@ private:
void exportToOPL(QString fileName);
void exportToCSV(QString fileName);
void exportToXML(QString fileName);
};
#endif // FLIGHTLOGMANAGER_H

4
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui
View File

@ -52,12 +52,12 @@
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBoxArmed">
<widget class="QCheckBox" name="checkBoxArming">
<property name="enabled">
<bool>true</bool>
</property>
<property name="text">
<string>Armed</string>
<string>Arm switch (Accessory0)</string>
</property>
</widget>
</item>

89
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp
View File

@ -94,12 +94,12 @@ ManualControlCommand *GCSControlGadget::getManualControlCommand()
return dynamic_cast<ManualControlCommand *>(objManager->getObject(QString("ManualControlCommand")));
}
void GCSControlGadget::manualControlCommandUpdated(UAVObject *obj)
void GCSControlGadget::manualControlCommandUpdated(UAVObject *manualControlCommand)
{
double roll = obj->getField("Roll")->getDouble();
double pitch = obj->getField("Pitch")->getDouble();
double yaw = obj->getField("Yaw")->getDouble();
double throttle = obj->getField("Throttle")->getDouble();
double roll = manualControlCommand->getField("Roll")->getDouble();
double pitch = manualControlCommand->getField("Pitch")->getDouble();
double yaw = manualControlCommand->getField("Yaw")->getDouble();
double throttle = manualControlCommand->getField("Throttle")->getDouble();
// necessary against having the wrong joystick profile chosen, which shows weird values
if (throttle > -1.0 && throttle <= 1.0) {
@ -140,11 +140,11 @@ void GCSControlGadget::manualControlCommandUpdated(UAVObject *obj)
*/
void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double rightX, double rightY)
{
ManualControlCommand *obj = getManualControlCommand();
double oldRoll = obj->getField("Roll")->getDouble();
double oldPitch = obj->getField("Pitch")->getDouble();
double oldYaw = obj->getField("Yaw")->getDouble();
double oldThrottle = obj->getField("Throttle")->getDouble();
ManualControlCommand *manualControlCommand = getManualControlCommand();
double oldRoll = manualControlCommand->getField("Roll")->getDouble();
double oldPitch = manualControlCommand->getField("Pitch")->getDouble();
double oldYaw = manualControlCommand->getField("Yaw")->getDouble();
double oldThrottle = manualControlCommand->getField("Throttle")->getDouble();
double newRoll = 0.0;
double newPitch = 0.0;
@ -221,18 +221,20 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r
if ((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) {
if (buttonRollControl == 0) {
obj->getField("Roll")->setDouble(newRoll);
manualControlCommand->getField("Roll")->setDouble(newRoll);
}
if (buttonPitchControl == 0) {
obj->getField("Pitch")->setDouble(newPitch);
manualControlCommand->getField("Pitch")->setDouble(newPitch);
}
if (buttonYawControl == 0) {
obj->getField("Yaw")->setDouble(newYaw);
manualControlCommand->getField("Yaw")->setDouble(newYaw);
}
if (buttonThrottleControl == 0) {
obj->getField("Throttle")->setDouble(newThrottle);
manualControlCommand->getField("Throttle")->setDouble(newThrottle);
manualControlCommand->getField("Thrust")->setDouble(newThrottle);
}
obj->updated();
manualControlCommand->getField("Connected")->setValue("True");
manualControlCommand->updated();
}
}
@ -294,27 +296,29 @@ void GCSControlGadget::readUDPCommand()
}
}
if (!badPack && ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) {
ManualControlCommand *obj = getManualControlCommand();
ManualControlCommand *manualControlCommand = getManualControlCommand();
bool update = false;
if (pitch != obj->getField("Pitch")->getDouble()) {
obj->getField("Pitch")->setDouble(constrain(pitch));
if (pitch != manualControlCommand->getField("Pitch")->getDouble()) {
manualControlCommand->getField("Pitch")->setDouble(constrain(pitch));
update = true;
}
if (yaw != obj->getField("Yaw")->getDouble()) {
obj->getField("Yaw")->setDouble(constrain(yaw));
if (yaw != manualControlCommand->getField("Yaw")->getDouble()) {
manualControlCommand->getField("Yaw")->setDouble(constrain(yaw));
update = true;
}
if (roll != obj->getField("Roll")->getDouble()) {
obj->getField("Roll")->setDouble(constrain(roll));
if (roll != manualControlCommand->getField("Roll")->getDouble()) {
manualControlCommand->getField("Roll")->setDouble(constrain(roll));
update = true;
}
if (throttle != obj->getField("Throttle")->getDouble()) {
obj->getField("Throttle")->setDouble(constrain(throttle));
if (throttle != manualControlCommand->getField("Throttle")->getDouble()) {
manualControlCommand->getField("Throttle")->setDouble(constrain(throttle));
manualControlCommand->getField("Thrust")->setDouble(constrain(throttle));
update = true;
}
if (update) {
obj->updated();
manualControlCommand->getField("Connected")->setValue("True");
manualControlCommand->updated();
}
}
}
@ -336,9 +340,9 @@ double GCSControlGadget::constrain(double value)
void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
{
if ((buttonSettings[number].ActionID > 0) && (buttonSettings[number].FunctionID > 0) && (pressed)) { // this button is configured
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
UAVDataObject *manualControlCommand = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
bool currentCGSControl = ((GCSControlGadgetWidget *)m_widget)->getGCSControl();
bool currentUDPControl = ((GCSControlGadgetWidget *)m_widget)->getUDPControl();
@ -347,16 +351,17 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
if (currentCGSControl) {
switch (buttonSettings[number].FunctionID) {
case 1: // Roll
obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble() + buttonSettings[number].Amount));
manualControlCommand->getField("Roll")->setValue(bound(manualControlCommand->getField("Roll")->getValue().toDouble() + buttonSettings[number].Amount));
break;
case 2: // Pitch
obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble() + buttonSettings[number].Amount));
manualControlCommand->getField("Pitch")->setValue(bound(manualControlCommand->getField("Pitch")->getValue().toDouble() + buttonSettings[number].Amount));
break;
case 3: // Yaw
obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble() + buttonSettings[number].Amount));
manualControlCommand->getField("Yaw")->setValue(wrap(manualControlCommand->getField("Yaw")->getValue().toDouble() + buttonSettings[number].Amount));
break;
case 4: // Throttle
obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() + buttonSettings[number].Amount));
manualControlCommand->getField("Throttle")->setValue(bound(manualControlCommand->getField("Throttle")->getValue().toDouble() + buttonSettings[number].Amount));
manualControlCommand->getField("Thrust")->setValue(bound(manualControlCommand->getField("Thrust")->getValue().toDouble() + buttonSettings[number].Amount));
break;
}
}
@ -365,16 +370,17 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
if (currentCGSControl) {
switch (buttonSettings[number].FunctionID) {
case 1: // Roll
obj->getField("Roll")->setValue(bound(obj->getField("Roll")->getValue().toDouble() - buttonSettings[number].Amount));
manualControlCommand->getField("Roll")->setValue(bound(manualControlCommand->getField("Roll")->getValue().toDouble() - buttonSettings[number].Amount));
break;
case 2: // Pitch
obj->getField("Pitch")->setValue(bound(obj->getField("Pitch")->getValue().toDouble() - buttonSettings[number].Amount));
manualControlCommand->getField("Pitch")->setValue(bound(manualControlCommand->getField("Pitch")->getValue().toDouble() - buttonSettings[number].Amount));
break;
case 3: // Yaw
obj->getField("Yaw")->setValue(wrap(obj->getField("Yaw")->getValue().toDouble() - buttonSettings[number].Amount));
manualControlCommand->getField("Yaw")->setValue(wrap(manualControlCommand->getField("Yaw")->getValue().toDouble() - buttonSettings[number].Amount));
break;
case 4: // Throttle
obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() - buttonSettings[number].Amount));
manualControlCommand->getField("Throttle")->setValue(bound(manualControlCommand->getField("Throttle")->getValue().toDouble() - buttonSettings[number].Amount));
manualControlCommand->getField("Thrust")->setValue(bound(manualControlCommand->getField("Thrust")->getValue().toDouble() - buttonSettings[number].Amount));
break;
}
}
@ -383,14 +389,10 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
switch (buttonSettings[number].FunctionID) {
case 1: // Armed
if (currentCGSControl) {
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
if (obj->getField("Armed")->getValue().toString().compare("Armed") == 0) {
obj->getField("Armed")->setValue("Disarmed");
if (((GCSControlGadgetWidget *)m_widget)->getArmed()) {
((GCSControlGadgetWidget *)m_widget)->setArmed(false);
} else {
obj->getField("Armed")->setValue("Armed");
((GCSControlGadgetWidget *)m_widget)->setArmed(true);
}
}
break;
@ -410,7 +412,8 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
break;
}
obj->updated();
manualControlCommand->getField("Connected")->setValue("True");
manualControlCommand->updated();
}
// buttonSettings[number].ActionID NIDT
// buttonSettings[number].FunctionID -RPYTAC

70
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp
View File

@ -46,15 +46,20 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
m_gcscontrol->setupUi(this);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
UAVObject::Metadata mdata = obj->getMetadata();
UAVDataObject *manualControlCommand = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
UAVObject::Metadata mdata = manualControlCommand->getMetadata();
m_gcscontrol->checkBoxGcsControl->setChecked(UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READONLY);
// Set up the drop down box for the flightmode
UAVDataObject *flightStatus = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
m_gcscontrol->comboBoxFlightMode->addItems(flightStatus->getField("FlightMode")->getOptions());
// TODO: update this with named modes based on current configuration
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 1");
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 2");
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 3");
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 4");
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 5");
m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 6");
// Set up slots and signals for joysticks
connect(m_gcscontrol->widgetLeftStick, SIGNAL(positionClicked(double, double)), this, SLOT(leftStickClicked(double, double)));
@ -62,13 +67,13 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
// Connect misc controls
connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(toggleControl(int)));
connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
connect(m_gcscontrol->checkBoxArming, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int)));
connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)), this, SLOT(toggleUDPControl(int))); // UDP control checkbox
// Connect object updated event from UAVObject to also update check boxes and dropdown
connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(mccChanged(UAVObject *)));
connect(manualControlCommand, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(mccChanged(UAVObject *)));
leftX = 0;
leftY = 0;
@ -115,18 +120,19 @@ void GCSControlGadgetWidget::rightStickClicked(double X, double Y)
*/
void GCSControlGadgetWidget::toggleControl(int state)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
UAVDataObject *manualControlCommand = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
UAVDataObject *accessoryDesired = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("AccessoryDesired"), 0));
UAVObject::Metadata mdata = obj->getMetadata();
UAVObject::Metadata mdata = manualControlCommand->getMetadata();
if (state) {
mccInitialData = mdata;
UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY);
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL);
UAVObject::SetGcsTelemetryAcked(mdata, false);
UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.gcsTelemetryUpdatePeriod = 100;
m_gcscontrol->checkBoxUDPControl->setEnabled(true);
} else {
@ -134,32 +140,27 @@ void GCSControlGadgetWidget::toggleControl(int state)
toggleUDPControl(false);
m_gcscontrol->checkBoxUDPControl->setEnabled(false);
}
obj->setMetadata(mdata);
manualControlCommand->setMetadata(mdata);
accessoryDesired->setMetadata(mdata);
}
void GCSControlGadgetWidget::toggleArmed(int state)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
UAVDataObject *accessoryDesired = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("AccessoryDesired"), 0));
if (state) {
obj->getField("Armed")->setValue("Armed");
accessoryDesired->getField("AccessoryVal")->setValue(1);
} else {
obj->getField("Armed")->setValue("Disarmed");
accessoryDesired->getField("AccessoryVal")->setValue(-1);
}
obj->updated();
accessoryDesired->updated();
}
void GCSControlGadgetWidget::mccChanged(UAVObject *obj)
void GCSControlGadgetWidget::mccChanged(UAVObject *manualControlCommand)
{
Q_UNUSED(obj);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *flightStatus = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
m_gcscontrol->comboBoxFlightMode->setCurrentIndex(m_gcscontrol->comboBoxFlightMode->findText(flightStatus->getField("FlightMode")->getValue().toString()));
m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed");
m_gcscontrol->comboBoxFlightMode->setCurrentIndex(manualControlCommand->getField("FlightModeSwitchPosition")->getValue().toInt());
}
void GCSControlGadgetWidget::toggleUDPControl(int state)
@ -176,13 +177,12 @@ void GCSControlGadgetWidget::toggleUDPControl(int state)
*/
void GCSControlGadgetWidget::selectFlightMode(int state)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
UAVObjectField *field = obj->getField("FlightMode");
UAVDataObject *manualControlCommand = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
field->setValue(field->getOptions()[state]);
obj->updated();
manualControlCommand->getField("FlightModeSwitchPosition")->setValue(state);
manualControlCommand->updated();
}
void GCSControlGadgetWidget::setGCSControl(bool newState)
@ -204,6 +204,16 @@ bool GCSControlGadgetWidget::getUDPControl(void)
return m_gcscontrol->checkBoxUDPControl->isChecked();
}
void GCSControlGadgetWidget::setArmed(bool newState)
{
m_gcscontrol->checkBoxArming->setChecked(newState);
}
bool GCSControlGadgetWidget::getArmed(void)
{
return m_gcscontrol->checkBoxArming->isChecked();
}
/**
* @}

2
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h
View File

@ -45,6 +45,8 @@ public:
bool getGCSControl(void);
void setUDPControl(bool newState);
bool getUDPControl(void);
void setArmed(bool newState);
bool getArmed(void);
signals:
void sticksChanged(double leftX, double leftY, double rightX, double rightY);

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"
#include "accelgyrosettings.h"
@ -306,29 +307,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()

0
ground/openpilotgcs/src/plugins/systemhealth/html/ManualControl-Critical.html → ground/openpilotgcs/src/plugins/systemhealth/html/Receiver-Critical.html
View File

0
ground/openpilotgcs/src/plugins/systemhealth/html/ManualControl-Warning.html → ground/openpilotgcs/src/plugins/systemhealth/html/Receiver-Warning.html
View File

0
ground/openpilotgcs/src/plugins/systemhealth/html/fr/ManualControl-Critical.html → ground/openpilotgcs/src/plugins/systemhealth/html/fr/Receiver-Critical.html
View File

0
ground/openpilotgcs/src/plugins/systemhealth/html/fr/ManualControl-Warning.html → ground/openpilotgcs/src/plugins/systemhealth/html/fr/Receiver-Warning.html
View File

8
ground/openpilotgcs/src/plugins/systemhealth/systemhealth.qrc
View File

@ -1,8 +1,8 @@
<RCC>
<qresource prefix="/systemhealth">
<file>html/Actuator-Critical.html</file>
<file>html/ManualControl-Critical.html</file>
<file>html/ManualControl-Warning.html</file>
<file>html/Receiver-Critical.html</file>
<file>html/Receiver-Warning.html</file>
<file>html/CPU-Critical.html</file>
<file>html/CPU-Warning.html</file>
<file>html/FlightTime-Error.html</file>
@ -29,8 +29,8 @@
</qresource>
<qresource prefix="/systemhealth" lang="fr">
<file alias="html/Actuator-Critical.html">html/fr/Actuator-Critical.html</file>
<file alias="html/ManualControl-Critical.html">html/fr/ManualControl-Critical.html</file>
<file alias="html/ManualControl-Warning.html">html/fr/ManualControl-Warning.html</file>
<file alias="html/Receiver-Critical.html">html/fr/Receiver-Critical.html</file>
<file alias="html/Receiver-Warning.html">html/fr/Receiver-Warning.html</file>
<file alias="html/CPU-Critical.html">html/fr/CPU-Critical.html</file>
<file alias="html/CPU-Warning.html">html/fr/CPU-Warning.html</file>
<file alias="html/FlightTime-Error.html">html/fr/FlightTime-Error.html</file>

2
ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp
View File

@ -525,7 +525,7 @@ void UAVObject::toXML(QXmlStreamWriter *xmlWriter)
xmlWriter->writeAttribute("name", getName());
xmlWriter->writeAttribute("id", QString("%1-%2").arg(getObjID(), 1, 16).toUpper().arg(getInstID()));
xmlWriter->writeStartElement("fields");
foreach(UAVObjectField* field , fields) {
foreach(UAVObjectField * field, fields) {
field->toXML(xmlWriter);
}
xmlWriter->writeEndElement(); // fields

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp
View File

@ -664,7 +664,7 @@ void UAVObjectField::toXML(QXmlStreamWriter *xmlWriter)
}
for (unsigned int n = 0; n < numElements; ++n) {
xmlWriter->writeStartElement("value");
if(getElementNames().size() > 1) {
if (getElementNames().size() > 1) {
xmlWriter->writeAttribute("name", getElementNames().at(n));
}
xmlWriter->writeCharacters(getValue(n).toString());

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

@ -72,6 +72,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 \
@ -172,6 +173,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"/>

1
shared/uavobjectdefinition/actuatorsettings.xml
View File

@ -8,6 +8,7 @@
<field name="ChannelType" units="" type="enum" elements="12" options="PWM,MK,ASTEC4,PWM Alarm Buzzer,Arming led,Info led" defaultvalue="PWM"/>
<field name="ChannelAddr" units="" type="uint8" elements="12" defaultvalue="0,1,2,3,4,5,6,7,8,9,10,11"/>
<field name="MotorsSpinWhileArmed" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

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"/>

3
shared/uavobjectdefinition/callbackinfo.xml
View File

@ -8,6 +8,7 @@
<elementname>AltitudeHold</elementname>
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
</elementnames>
</field>
<field name="Running" units="bool" type="enum">
@ -17,6 +18,7 @@
<elementname>AltitudeHold</elementname>
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
</elementnames>
<options>
<option>False</option>
@ -30,6 +32,7 @@
<elementname>AltitudeHold</elementname>
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
</elementnames>
</field>
<access gcs="readonly" flight="readwrite"/>

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>

8
shared/uavobjectdefinition/flightstatus.xml
View File

@ -6,6 +6,14 @@
<!-- Note these enumerated values should be the same as ManualControlSettings -->
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,AltitudeVario,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
<field name="ControlChain" units="bool" type="enum" options="false,true">
<elementnames>
<elementname>Stabilization</elementname>
<elementname>PathFollower</elementname>
<elementname>PathPlanner</elementname>
</elementnames>
</field>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="onchange" period="5000"/>

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"/>

59
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -13,68 +13,17 @@
<field name="ChannelMax" units="us" type="int16" defaultvalue="2000"
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/>
<field name="ResponseTime" units="ms" type="uint16" defaultvalue="0"
elementnames="Roll,Pitch,Yaw,Accessory0,Accessory1,Accessory2"/>
elementnames="Roll,Pitch,Yaw,Collective,Accessory0,Accessory1,Accessory2"/>
<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"/>

1
shared/uavobjectdefinition/mixersettings.xml
View File

@ -26,6 +26,7 @@
<options>
<option>Disabled</option>
<option>Motor</option>
<option>ReversableMotor</option>
<option>Servo</option>
<option>CameraRollOrServo1</option>
<option>CameraPitchOrServo2</option>

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"/>

6
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"/>
@ -41,8 +41,6 @@
<field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>
<field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>
<field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>

1
shared/uavobjectdefinition/systemalarms.xml
View File

@ -10,6 +10,7 @@
<elementname>CPUOverload</elementname>
<elementname>EventSystem</elementname>
<elementname>Telemetry</elementname>
<elementname>Receiver</elementname>
<elementname>ManualControl</elementname>
<elementname>Actuator</elementname>
<elementname>Attitude</elementname>

13
shared/uavobjectdefinition/systemsettings.xml
View File

@ -2,6 +2,19 @@
<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 -->

6
shared/uavobjectdefinition/taskinfo.xml
View File

@ -10,7 +10,7 @@
<elementname>CallbackScheduler2</elementname>
<elementname>CallbackScheduler3</elementname>
<!-- fligth -->
<elementname>ManualControl</elementname>
<elementname>Receiver</elementname>
<elementname>Stabilization</elementname>
<elementname>Actuator</elementname>
<elementname>Sensors</elementname>
@ -43,7 +43,7 @@
<elementname>CallbackScheduler2</elementname>
<elementname>CallbackScheduler3</elementname>
<!-- fligth -->
<elementname>ManualControl</elementname>
<elementname>Receiver</elementname>
<elementname>Stabilization</elementname>
<elementname>Actuator</elementname>
<elementname>Sensors</elementname>
@ -81,7 +81,7 @@
<elementname>CallbackScheduler2</elementname>
<elementname>CallbackScheduler3</elementname>
<!-- fligth -->
<elementname>ManualControl</elementname>
<elementname>Receiver</elementname>
<elementname>Stabilization</elementname>
<elementname>Actuator</elementname>
<elementname>Sensors</elementname>

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"/>