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

Merge remote-tracking branch 'origin/next' into filnet/OP-1194_persist_scope_gadget_plot_and_legend_visibility_state

Browse Source
This commit is contained in:
Philippe Renon 2014-03-20 01:42:26 +01:00
commit 03bdd5e03e
1719 changed files with 225946 additions and 39724 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
WHATSNEW.txt
View File

@ -1,5 +1,5 @@
--- RELEASE-14.01-RC1 --- Cruising Ratt ---
This is the RC1 for the first 2014 software release.
--- RELEASE-14.01 --- Cruising Ratt ---
This is the first 2014 software release.
This version still supports the CopterControl and CC3D.
It includes some major "under the hood" changes like migration
to Qt5.1 and QtQuick2 widgets, an overhaul of UAVTalk to improve
@ -13,7 +13,119 @@ Some additions in this release:
the full list of features, improvements and bufixes shipping
in this release is accessible here:
http://progress.openpilot.org/browse/OP/fixforversion/10220
http://progress.openpilot.org/issues/?filter=11260
** Improvement
* [OP-771] - Change Wizard wording for better usability
* [OP-791] - Integrate About Authors, OpenPilot GCS, Plugins dialogs into a single dialog window
* [OP-803] - Gadgets get their configuration set twice when restoring workspaces during GCS startup
* [OP-835] - Upgrade GCS to use Qt 5.1.0
* [OP-883] - Make system and flight targets cleanup, pass 01
* [OP-913] - Poor UAVObject data structure alignment on flight side causes performance degradation
* [OP-951] - Add -Wshadow to flight CFLAGS and fix compilation breakage that results
* [OP-966] - Scope Plugin Cleanup
* [OP-984] - Provide multi PID banks, these should be assignable per flight mode.
* [OP-996] - Add GCS option to remember the last selected workspace
* [OP-1022] - Additional improvements for altitude hold
* [OP-1036] - Improvements to Fixed Wing PathFollower and Nav
* [OP-1059] - Typo (2x) in OpenPilot Setup Wizard - Output Calibration Window
* [OP-1063] - Multirotor Configuration
* [OP-1071] - Make map "emergency" lines less strong and dashed
* [OP-1079] - Update to FreeRTOS v7.5.2
* [OP-1082] - Add a ticker on the Welcome page showing Jira activity alongside the 'Project News'
* [OP-1083] - Fix minor English spelling errors in stabilization tooltips
* [OP-1085] - Upgrade GCS to use Qt 5.1.1
* [OP-1094] - Turn on Progress for large SDK downloads / remove for MD5 files
* [OP-1104] - Create BL version 6 to support larger firmware
* [OP-1105] - If firmware .info blob is missing, test string is too long
* [OP-1107] - Convert About dialog to QTQuick 2.0 and cleanup code.
* [OP-1110] - Move Welcome screen to QtQuick 2
* [OP-1111] - Move About to QtQuick2
* [OP-1112] - Update contributors in GCS
* [OP-1113] - Convert new PFD design to QtQuik2
* [OP-1117] - Implement Horizon mode
* [OP-1120] - Waypoint upload to board should be transacted
* [OP-1133] - UAVTalk - expose send/request all instances of multi instance uav objects + related uavtalk fixes
* [OP-1137] - Make Configuration Checkbox checked by default during uninstall
* [OP-1141] - Add a further bias correction to barometer to better handle thermal variations
* [OP-1143] - Missing Linux udev rules for Revolution boards
* [OP-1153] - Provide a mean to instrument SystemMod stack utilization
* [OP-1154] - Config Option to Automatically Increase Copter Throttle per 1/cos(bank_angle)
* [OP-1158] - Add flight plan consistency checks
* [OP-1160] - Some dev Env improvements, git hooks for messages, make prepare etc.
** Task
* [OP-775] - Add ARM DSP library to OP codebase
* [OP-813] - Manage merge of translation work to French
* [OP-839] - Disable pyMyte dependency until really used
* [OP-901] - Update STM32 StdPeriphLib to current
* [OP-1087] - Update Qt used from Makefile to 5.1.1 for Windows and Mac
* [OP-1109] - Created share Qt5 QtQuick2 port branch
* [OP-1115] - Remove old artwork from the Artwork folder in Git
* [OP-1119] - Write GCS plugin to access and display on board logs through uavtalk and export .opl files from logged uavobjects
* [OP-1058] - UAVO:Implement a structured named accessors for multielement fields (Flight side)
** Bug
* [OP-844] - Fix header comments in altitudehold.c
* [OP-845] - Fix reading serial number from USB device on mac platform
* [OP-846] - make qt_sdk_install fails
* [OP-865] - PWM output 6 does not work on RM
* [OP-887] - Provide some standard method of calibrating CPU speed and load measurement for boards
* [OP-924] - PPM output does not have failsafe
* [OP-934] - Incorrect timeout handling in rfm22b receiver
* [OP-971] - Add UI to set AccellTau with revo board
* [OP-1004] - UAVObjectBrowser, buttons don't work when scientific display is turned on
* [OP-1014] - Com port connections are not working on OPLink
* [OP-1018] - Zero point initialization in ETASv3 Airspeed sensor buggy
* [OP-1027] - Segfault in UAVObjectBrowser when "Request"ing a UAVObjectCategory
* [OP-1042] - Revo firmware version isn't read correctly through OPLink
* [OP-1046] - Waypoint upload incomplete, no visual confirmation of failed uploads in uavobjectbrowser and waypoint editor
* [OP-1048] - Attitude is not working with AccelTau > 0
* [OP-1049] - CC3D attitude estimation failure after multiple settings changes and reboots
* [OP-1067] - Invalid value for "LinkState"
* [OP-1076] - CF Attitude filter in next randomly re-initializes on arming.
* [OP-1078] - GCS segfaults if you close it after playing a log file
* [OP-1080] - Unreliable detection of board through OPLink
* [OP-1095] - GCS crashing on macosx 10.9 upon connection of oplink mini
* [OP-1098] - CDC driver fails installation in Windows 8 or 8.1
* [OP-1099] - Hidden icons in Configuration tab
* [OP-1101] - Tools.mk has a few tabs and they need to be converted to spaces
* [OP-1102] - OP GCS registers some file types is should not
* [OP-1103] - GCS can not be compiled on OSX 10.8 after update to Qt5.1.1
* [OP-1108] - Minor bugs found while reading the code
* [OP-1114] - QGLWidget prohibits QListWidgetItem, set AA_DontCreateNativeWidgetSiblings as work around
* [OP-1118] - QComboBox in UAVObjectBrowser does not stay in focus on Mac OSX
* [OP-1121] - GCS will not exit if the Waypoint editor/PathPlanner dialog is open
* [OP-1123] - GCS assertion failure when loading a waypoint file
* [OP-1125] - UAVTalk - acking/nacking multi instance uavobjects is broken (when sending individual instances)
* [OP-1132] - LIBEAY32.dll missing from installer
* [OP-1139] - Add higher order correction to MS5611 driver for low and very low temperature compensation
* [OP-1142] - No yaw in Horizon mode
* [OP-1145] - OPLM to GCS link not reliable
* [OP-1148] - Futaba R7008SB S.Bus protocol not supported
* [OP-1151] - PFD display - inverted flight
* [OP-1152] - Check Stack usage for CopterControl & CC3D
* [OP-1155] - Fix OSX Packaging for GCS
* [OP-1157] - sin_lookup_deg() returns garbage for negative angles
* [OP-1166] - GCS misses yaw neutral setting on sync from initial connection
* [OP-1167] - New flight mode switch position UAVO to work better with SITL, HITL
* [OP-1168] - GCS Reload Board Data button doesn't work
* [OP-1169] - GCS UAVO object titles off by one
* [OP-1176] - Cruise Control checkboxes use wrong Default button
* [OP-1177] - AltHold - Need a setting to allow disabling of bank angle throttle compensation in AH
* [OP-1178] - After re-factoring of ConfigTaskWidget code the OPLink config page does not work reliably.
* [OP-1179] - About box not working in Linux64 build (but probably the same is for Linux32)
* [OP-1180] - GCS AltHold Tab - Reload button and update in real time
* [OP-1181] - on radio configuration the pitch slider has maxed out on its own three times randomly
* [OP-1182] - Telemetry monitor widget is too small on Mac
* [OP-1183] - UAVBrowser displays hex string as decimal
* [OP-1184] - Scope gadget - Stack monitor configurations need a cleanup
* [OP-1188] - Optimize Stabilization Module stack size usage
* [OP-1191] - Revo OPLink bug in GCS
* [OP-1192] - Even though Throttle is off there is motor movement in some situations.
* [OP-1211] - dT calculation in Stabilization and other modules unsafe
* [OP-1218] - PIOS_COM is not thread safe
* [OP-1228] - GCS Quits unexpectedly
--- RELEASE-13.06.04 ---
This maintenance release includes the following fixes missing in (previously not released to public) RELEASE-13.06.03.

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;

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

@ -55,7 +55,7 @@
#define STACK_SIZE_BYTES 1312
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // device driver
#define FAILSAFE_TIMEOUT_MS 100
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
@ -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;

16
flight/modules/Altitude/revolution/altitude.c
View File

@ -55,6 +55,8 @@
// Private variables
static xTaskHandle taskHandle;
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool tempCorrectionEnabled;
// Private functions
static void altitudeTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev);
@ -81,7 +83,7 @@ int32_t AltitudeInitialize()
BaroSensorInitialize();
RevoSettingsInitialize();
RevoSettingsConnectCallback(&SettingsUpdatedCb);
SettingsUpdatedCb(NULL);
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
#endif
@ -163,9 +165,14 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure();
float temp2 = temp * temp;
press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d;
if (tempCorrectionEnabled) {
// pressure bias = A + B*t + C*t^2 + D * t^3
// in case the temperature is outside of the calibrated range, uses the nearest extremes
float ctemp = temp > baroCorrectionExtent.max ? baroCorrectionExtent.max :
(temp < baroCorrectionExtent.min ? baroCorrectionExtent.min : temp);
press -= baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
}
float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
if (!isnan(altitude)) {
@ -181,6 +188,9 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
tempCorrectionEnabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
(baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
}
/**
* @}

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

@ -45,6 +45,8 @@
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
@ -84,7 +86,7 @@ int32_t AltitudeHoldStart()
{
// Start main task
SettingsUpdatedCb(NULL);
DelayedCallbackDispatch(altitudeHoldCBInfo);
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
return 0;
}
@ -101,7 +103,7 @@ int32_t AltitudeHoldInitialize()
// Create object queue
altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES);
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
return 0;
@ -114,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;
@ -127,8 +129,8 @@ static void altitudeHoldTask(void)
default:
pid_zero(&pid0);
pid_zero(&pid1);
StabilizationDesiredThrottleGet(&startThrottle);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
StabilizationDesiredThrustGet(&startThrust);
PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
break;
@ -160,37 +162,37 @@ 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;
StabilizationDesiredSet(&stab);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)

169
flight/modules/Attitude/attitude.c
View File

@ -57,6 +57,7 @@
#include "accelstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "accelgyrosettings.h"
#include "flightstatus.h"
#include "manualcontrolcommand.h"
#include "taskinfo.h"
@ -70,12 +71,17 @@
#define SENSOR_PERIOD 4
#define UPDATE_RATE 25.0f
#define GYRO_NEUTRAL 1665
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
// Private types
// Private variables
static xTaskHandle taskHandle;
static PiOSDeltatimeConfig dtconfig;
// Private functions
static void AttitudeTask(void *parameters);
@ -96,24 +102,42 @@ static float accels_filtered[3];
static float grot_filtered[3];
static float yawBiasRate = 0;
static float rollPitchBiasRate = 0.0f;
static float gyroGain = 0.42f;
static int16_t accelbias[3];
static AccelGyroSettingsaccel_biasData accel_bias;
static float q[4] = { 1, 0, 0, 0 };
static float R[3][3];
static int8_t rotate = 0;
static bool zero_during_arming = false;
static bool bias_correct_gyro = true;
// static float gyros_passed[3];
// temp coefficient to calculate gyro bias
static bool apply_gyro_temp = false;
static bool apply_accel_temp = false;
static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;;
static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff;
static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent;
// Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings
static AccelGyroSettingsgyro_scaleData gyro_scale;
static AccelGyroSettingsaccel_scaleData accel_scale;
// For running trim flights
static volatile bool trim_requested = false;
static volatile int32_t trim_accels[3];
static volatile int32_t trim_samples;
int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define GRAV 9.81f
#define ACCEL_SCALE (GRAV * 0.004f)
#define GRAV 9.81f
#define STD_CC_ACCEL_SCALE (GRAV * 0.004f)
/* 0.004f is gravity / LSB */
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665
#define STD_CC_ANALOG_GYRO_GAIN 0.42f
// Used to detect CC vs CC3D
static const struct pios_board_info *bdinfo = &pios_board_info_blob;
#define BOARDISCC3D (bdinfo->board_rev == 0x02)
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
@ -136,6 +160,7 @@ int32_t AttitudeInitialize(void)
{
AttitudeStateInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
AccelStateInitialize();
GyroStateInitialize();
@ -166,7 +191,7 @@ int32_t AttitudeInitialize(void)
trim_requested = false;
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -190,9 +215,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
const struct pios_board_info *bdinfo = &pios_board_info_blob;
bool cc3d = bdinfo->board_rev == 0x02;
bool cc3d = BOARDISCC3D;
if (cc3d) {
#if defined(PIOS_INCLUDE_MPU6000)
@ -214,6 +237,8 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
// Main task loop
while (1) {
FlightStatusData flightStatus;
@ -272,8 +297,6 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
}
}
float gyros_passed[3];
/**
* Get an update from the sensors
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
@ -301,9 +324,9 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
}
// First sample is temperature
gyros->x = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
gyros->y = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
gyros->z = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
gyros->x = -(gyro[1] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.X;
gyros->y = (gyro[2] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Y;
gyros->z = -(gyro[3] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Z;
int32_t x = 0;
int32_t y = 0;
@ -317,9 +340,10 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
y += -accel_data.y;
z += -accel_data.z;
} while ((i < 32) && (samples_remaining > 0));
// gyros->temperature = samples_remaining;
float accel[3] = { (float)x / i, (float)y / i, (float)z / i };
float accel[3] = { accel_scale.X * (float)x / i,
accel_scale.Y * (float)y / i,
accel_scale.Z * (float)z / i };
if (rotate) {
// TODO: rotate sensors too so stabilization is well behaved
@ -357,9 +381,9 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
}
// Scale accels and correct bias
accelState->x = (accelState->x - accelbias[0]) * ACCEL_SCALE;
accelState->y = (accelState->y - accelbias[1]) * ACCEL_SCALE;
accelState->z = (accelState->z - accelbias[2]) * ACCEL_SCALE;
accelState->x -= accel_bias.X;
accelState->y -= accel_bias.Y;
accelState->z -= accel_bias.Z;
if (bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias
@ -387,7 +411,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not
*/
struct pios_mpu6000_data mpu6000_data;
static struct pios_mpu6000_data mpu6000_data;
static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
{
float accels[3], gyros[3];
@ -403,15 +427,30 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
if (GyroStateReadOnly() || AccelStateReadOnly()) {
return 0;
}
gyros[0] = mpu6000_data.gyro_x * gyro_scale.X;
gyros[1] = mpu6000_data.gyro_y * gyro_scale.Y;
gyros[2] = mpu6000_data.gyro_z * gyro_scale.Z;
gyros[0] = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
gyros[1] = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
gyros[2] = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();
accels[0] = mpu6000_data.accel_x * accel_scale.X;
accels[1] = mpu6000_data.accel_y * accel_scale.Y;
accels[2] = mpu6000_data.accel_z * accel_scale.Z;
accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale();
accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale();
accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale();
float ctemp = mpu6000_data.temperature > temp_calibrated_extent.max ? temp_calibrated_extent.max :
(mpu6000_data.temperature < temp_calibrated_extent.min ? temp_calibrated_extent.min
: mpu6000_data.temperature);
if (apply_gyro_temp) {
gyros[0] -= (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
gyros[1] -= (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros[2] -= (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (apply_accel_temp) {
accels[0] -= accel_temp_coeff.X * ctemp;
accels[1] -= accel_temp_coeff.Y * ctemp;
accels[2] -= accel_temp_coeff.Z * ctemp;
}
// gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
// accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
#endif /* if defined(PIOS_INCLUDE_MPU6000) */
@ -429,9 +468,9 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
gyros[2] = vec_out[2];
}
accelStateData->x = accels[0] - accelbias[0] * ACCEL_SCALE; // Applying arbitrary scale here to match CC v1
accelStateData->y = accels[1] - accelbias[1] * ACCEL_SCALE;
accelStateData->z = accels[2] - accelbias[2] * ACCEL_SCALE;
accelStateData->x = accels[0] - accel_bias.X;
accelStateData->y = accels[1] - accel_bias.Y;
accelStateData->z = accels[2] - accel_bias.Z;
gyrosData->x = gyros[0];
gyrosData->y = gyros[1];
@ -473,12 +512,7 @@ static inline void apply_accel_filter(const float *raw, float *filtered)
static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData)
{
float dT;
portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0;
dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f;
lastSysTime = thisSysTime;
float dT = PIOS_DELTATIME_GetAverageSeconds(&dtconfig);
// Bad practice to assume structure order, but saves memory
float *gyros = &gyrosData->x;
@ -587,14 +621,14 @@ static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosD
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{
AttitudeSettingsData attitudeSettings;
AccelGyroSettingsData accelGyroSettings;
AttitudeSettingsGet(&attitudeSettings);
AccelGyroSettingsGet(&accelGyroSettings);
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKi;
yawBiasRate = attitudeSettings.YawBiasRate;
gyroGain = attitudeSettings.GyroGain;
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0025f;
@ -609,13 +643,54 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
accelbias[0] = attitudeSettings.AccelBias.X;
accelbias[1] = attitudeSettings.AccelBias.Y;
accelbias[2] = attitudeSettings.AccelBias.Z;
memcpy(&gyro_temp_coeff, &accelGyroSettings.gyro_temp_coeff, sizeof(AccelGyroSettingsgyro_temp_coeffData));
memcpy(&accel_temp_coeff, &accelGyroSettings.accel_temp_coeff, sizeof(AccelGyroSettingsaccel_temp_coeffData));
gyro_correct_int[0] = attitudeSettings.GyroBias.X / 100.0f;
gyro_correct_int[1] = attitudeSettings.GyroBias.Y / 100.0f;
gyro_correct_int[2] = attitudeSettings.GyroBias.Z / 100.0f;
apply_gyro_temp = (fabsf(gyro_temp_coeff.X) > 1e-6f ||
fabsf(gyro_temp_coeff.Y) > 1e-6f ||
fabsf(gyro_temp_coeff.Z) > 1e-6f ||
fabsf(gyro_temp_coeff.X2) > 1e-6f ||
fabsf(gyro_temp_coeff.Y2) > 1e-6f ||
fabsf(gyro_temp_coeff.Z2) > 1e-6f);
apply_accel_temp = (fabsf(accel_temp_coeff.X) > 1e-6f ||
fabsf(accel_temp_coeff.Y) > 1e-6f ||
fabsf(accel_temp_coeff.Z) > 1e-6f);
gyro_correct_int[0] = accelGyroSettings.gyro_bias.X;
gyro_correct_int[1] = accelGyroSettings.gyro_bias.Y;
gyro_correct_int[2] = accelGyroSettings.gyro_bias.Z;
temp_calibrated_extent.min = accelGyroSettings.temp_calibrated_extent.min;
temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max;
if (BOARDISCC3D) {
accel_bias.X = accelGyroSettings.accel_bias.X;
accel_bias.Y = accelGyroSettings.accel_bias.Y;
accel_bias.Z = accelGyroSettings.accel_bias.Z;
gyro_scale.X = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale();
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale();
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale();
accel_scale.X = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale();
accel_scale.Y = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale();
accel_scale.Z = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale();
} else {
// Original CC with analog gyros and ADXL accel
accel_bias.X = accelGyroSettings.accel_bias.X;
accel_bias.Y = accelGyroSettings.accel_bias.Y;
accel_bias.Z = accelGyroSettings.accel_bias.Z;
gyro_scale.X = accelGyroSettings.gyro_scale.X * STD_CC_ANALOG_GYRO_GAIN;
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * STD_CC_ANALOG_GYRO_GAIN;
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * STD_CC_ANALOG_GYRO_GAIN;
accel_scale.X = accelGyroSettings.accel_scale.X * STD_CC_ACCEL_SCALE;
accel_scale.Y = accelGyroSettings.accel_scale.Y * STD_CC_ACCEL_SCALE;
accel_scale.Z = accelGyroSettings.accel_scale.Z * STD_CC_ACCEL_SCALE;
}
// Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 &&
@ -643,11 +718,11 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
trim_requested = true;
} else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
trim_requested = false;
attitudeSettings.AccelBias.X = trim_accels[0] / trim_samples;
attitudeSettings.AccelBias.Y = trim_accels[1] / trim_samples;
accelGyroSettings.accel_scale.X = trim_accels[0] / trim_samples;
accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples;
// Z should average -grav
attitudeSettings.AccelBias.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + GRAV;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings);
} else {
trim_requested = false;

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

52
flight/modules/CallbackTest/callbacktest.c
View File

@ -65,13 +65,13 @@ int32_t CallbackTestInitialize()
{
mutex = xSemaphoreCreateRecursiveMutex();
cbinfo[0] = DelayedCallbackCreate(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
cbinfo[1] = DelayedCallbackCreate(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
cbinfo[2] = DelayedCallbackCreate(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, STACK_SIZE);
cbinfo[3] = DelayedCallbackCreate(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, STACK_SIZE);
cbinfo[4] = DelayedCallbackCreate(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
cbinfo[5] = DelayedCallbackCreate(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
cbinfo[6] = DelayedCallbackCreate(&DelayedCb6, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 20, STACK_SIZE);
cbinfo[0] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
cbinfo[1] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
cbinfo[2] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
cbinfo[3] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
cbinfo[4] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
cbinfo[5] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
cbinfo[6] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb6, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 20, -1, STACK_SIZE);
return 0;
@ -79,22 +79,22 @@ int32_t CallbackTestInitialize()
int32_t CallbackTestStart()
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
DelayedCallbackDispatch(cbinfo[3]);
DelayedCallbackDispatch(cbinfo[2]);
DelayedCallbackDispatch(cbinfo[1]);
DelayedCallbackDispatch(cbinfo[0]);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[3]);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[2]);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[1]);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[0]);
// different callback priorities within a taskpriority
DelayedCallbackSchedule(cbinfo[4], 30000, CALLBACK_UPDATEMODE_NONE);
DelayedCallbackSchedule(cbinfo[4], 5000, CALLBACK_UPDATEMODE_OVERRIDE);
DelayedCallbackSchedule(cbinfo[4], 4000, CALLBACK_UPDATEMODE_SOONER);
DelayedCallbackSchedule(cbinfo[4], 10000, CALLBACK_UPDATEMODE_SOONER);
DelayedCallbackSchedule(cbinfo[4], 1000, CALLBACK_UPDATEMODE_LATER);
DelayedCallbackSchedule(cbinfo[4], 4800, CALLBACK_UPDATEMODE_LATER);
DelayedCallbackSchedule(cbinfo[4], 48000, CALLBACK_UPDATEMODE_NONE);
PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 30000, CALLBACK_UPDATEMODE_NONE);
PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 5000, CALLBACK_UPDATEMODE_OVERRIDE);
PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 4000, CALLBACK_UPDATEMODE_SOONER);
PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 10000, CALLBACK_UPDATEMODE_SOONER);
PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 1000, CALLBACK_UPDATEMODE_LATER);
PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 4800, CALLBACK_UPDATEMODE_LATER);
PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 48000, CALLBACK_UPDATEMODE_NONE);
// should be at 4.8 seconds after this, allowing for exactly 9 prints of the following
DelayedCallbackSchedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
// delayed counter with 500 ms
DelayedCallbackDispatch(cbinfo[6]);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[6]);
// high task prio
xSemaphoreGiveRecursive(mutex);
return 0;
@ -104,28 +104,28 @@ static void DelayedCb0()
{
DEBUGPRINT("delayed counter low prio 0 updated: %i\n", counter[0]);
if (++counter[0] < 10) {
DelayedCallbackDispatch(cbinfo[0]);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[0]);
}
}
static void DelayedCb1()
{
DEBUGPRINT("delayed counter low prio 1 updated: %i\n", counter[1]);
if (++counter[1] < 10) {
DelayedCallbackDispatch(cbinfo[1]);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[1]);
}
}
static void DelayedCb2()
{
DEBUGPRINT("delayed counter high prio 2 updated: %i\n", counter[2]);
if (++counter[2] < 10) {
DelayedCallbackDispatch(cbinfo[2]);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[2]);
}
}
static void DelayedCb3()
{
DEBUGPRINT("delayed counter high prio 3 updated: %i\n", counter[3]);
if (++counter[3] < 10) {
DelayedCallbackDispatch(cbinfo[3]);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[3]);
}
}
static void DelayedCb4()
@ -137,7 +137,7 @@ static void DelayedCb5()
{
DEBUGPRINT("delayed scheduled counter 5 updated: %i\n", counter[5]);
if (++counter[5] < 10) {
DelayedCallbackSchedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
}
// it will likely only reach 8 due to cb4 aborting the run
}
@ -145,6 +145,6 @@ static void DelayedCb6()
{
DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n", counter[6]);
if (++counter[6] < 10) {
DelayedCallbackDispatch(cbinfo[6]);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[6]);
}
}

1
flight/modules/CameraStab/camerastab.c
View File

@ -128,6 +128,7 @@ int32_t CameraStabInitialize(void)
.obj = AttitudeStateHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);

9
flight/modules/Example/examplemodcallback.c
View File

@ -45,6 +45,7 @@
*/
#include "openpilot.h"
#include "callbackinfo.h" // object needed for callback id macro CALLBACKINFO_RUNNING_<MODULENAME>
#include "exampleobject1.h" // object the module will listen for updates (input)
#include "exampleobject2.h" // object the module will update (output)
#include "examplesettings.h" // object holding module settings (input)
@ -71,7 +72,7 @@ int32_t ExampleModCallbackInitialize()
// Listen for ExampleObject1 updates, connect a callback function
ExampleObject1ConnectCallback(&ObjectUpdatedCb);
cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE);
cbinfo = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_EXAMPLE, STACK_SIZE);
return 0;
}
@ -85,11 +86,11 @@ int32_t ExampleModCallbackInitialize()
*/
static void ObjectUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(cbinfo);
PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo);
}
/**
* This function is called by the DelayedCallbackScheduler when its execution
* This function is called by the PIOS_CALLBACKSCHEDULER_Scheduler when its execution
* has been requested. Callbacks scheduled for execution are executed in the
* same thread in a round robin fashion. The Dispatch function to reschedule
* execution can be called from within the Callback itself, in which case the
@ -135,5 +136,5 @@ ExampleObject2Set(&data2);
// call the module again 10 seconds later,
// even if the exampleobject has not been updated
DelayedCallbackSchedule(cbinfo, 10 * 1000, CALLBACK_UPDATEMODE_NONE);
PIOS_CALLBACKSCHEDULER_Schedule(cbinfo, 10 * 1000, CALLBACK_UPDATEMODE_NONE);
}

2
flight/modules/FirmwareIAP/firmwareiap.c
View File

@ -100,6 +100,7 @@ int32_t FirmwareIAPInitialize()
PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
data.BoardRevision = bdinfo->board_rev;
data.BootloaderRevision = bdinfo->bl_rev;
data.ArmReset = 0;
data.crc = 0;
FirmwareIAPObjSet(&data);
@ -147,6 +148,7 @@ static void FirmwareIAPCallback(UAVObjEvent *ev)
PIOS_BL_HELPER_FLASH_Read_Description(data.Description, FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM);
PIOS_SYS_SerialNumberGetBinary(data.CPUSerial);
data.BoardRevision = bdinfo->board_rev;
data.BootloaderRevision = bdinfo->bl_rev;
data.crc = PIOS_BL_HELPER_CRC_Memory_Calc();
FirmwareIAPObjSet(&data);
}

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

7
flight/modules/Logging/Logging.c
View File

@ -73,7 +73,12 @@ int32_t LoggingStart(void)
FlightStatusConnectCallback(FlightStatusUpdatedCb);
SettingsUpdatedCb(DebugLogSettingsHandle());
UAVObjEvent ev = { .obj = DebugLogSettingsHandle(), .instId = 0, .event = EV_UPDATED_PERIODIC };
UAVObjEvent ev = {
.obj = DebugLogSettingsHandle(),
.instId = 0,
.event = EV_UPDATED_PERIODIC,
.lowPriority = true,
};
EventPeriodicCallbackCreate(&ev, StatusUpdatedCb, 1000);
// invoke a periodic dispatcher callback - the event struct is a dummy, it could be filled with anything!
StatusUpdatedCb(&ev);

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 + 4)
#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);
}
/**
* @}
* @}
*/

2
flight/modules/OPLink/oplinkmod.c
View File

@ -119,7 +119,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
MODULE_TASKCREATE_ALL;
/* start the delayed callback scheduler */
CallbackSchedulerStart();
PIOS_CALLBACKSCHEDULER_Start();
if (mallocFailed) {
/* We failed to malloc during task creation,

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

@ -31,6 +31,7 @@
#include "openpilot.h"
#include "callbackinfo.h"
#include "pathplan.h"
#include "flightstatus.h"
#include "airspeedstate.h"
@ -41,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"
@ -95,7 +96,7 @@ int32_t PathPlannerStart()
PathStatusConnectCallback(statusUpdated);
// Start main task callback
DelayedCallbackDispatch(pathPlannerHandle);
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
return 0;
}
@ -115,8 +116,8 @@ int32_t PathPlannerInitialize()
WaypointInitialize();
WaypointActiveInitialize();
pathPlannerHandle = DelayedCallbackCreate(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, STACK_SIZE_BYTES);
pathDesiredUpdaterHandle = DelayedCallbackCreate(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, STACK_SIZE_BYTES);
pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
return 0;
}
@ -128,7 +129,7 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
*/
static void pathPlannerTask()
{
DelayedCallbackSchedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
PIOS_CALLBACKSCHEDULER_Schedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
bool endCondition = false;
@ -138,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
@ -170,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;
@ -332,13 +333,13 @@ static uint8_t checkPathPlan()
// callback function when status changed, issue execution of state machine
void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(pathDesiredUpdaterHandle);
PIOS_CALLBACKSCHEDULER_Dispatch(pathDesiredUpdaterHandle);
}
// callback function when waypoints changed in any way, update pathDesired
void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(pathPlannerHandle);
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
}

26
flight/modules/RadioComBridge/RadioComBridge.c
View File

@ -268,6 +268,7 @@ static void registerObject(UAVObjHandle obj)
.obj = obj,
.instId = UAVOBJ_ALL_INSTANCES,
.event = EV_UPDATED_PERIODIC,
.lowPriority = true,
};
// Get metadata
@ -415,7 +416,11 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
// Send the data straight to the telemetry port.
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
int32_t ret = -2;
uint8_t count = 5;
while (count-- > 0 && ret < -1) {
ret = PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
}
}
}
} else {
@ -511,7 +516,11 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
// Send the data over the radio link.
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
int32_t ret = -2;
uint8_t count = 5;
while (count-- > 0 && ret < -1) {
ret = PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
}
}
} else {
vTaskDelay(5);
@ -541,7 +550,11 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
if (outputPort) {
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
ret = -2;
uint8_t count = 5;
while (count-- > 0 && ret < -1) {
ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
}
} else {
ret = -1;
}
@ -567,7 +580,12 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
if (outputPort && PIOS_COM_Available(outputPort)) {
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
int32_t ret = -2;
uint8_t count = 5;
while (count-- > 0 && ret < -1) {
ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
}
return ret;
} else {
return -1;
}

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

91
flight/modules/Sensors/sensors.c
View File

@ -55,6 +55,7 @@
#include <attitudestate.h>
#include <attitudesettings.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <flightstatus.h>
#include <taskinfo.h>
@ -78,16 +79,15 @@ static void settingsUpdatedCb(UAVObjEvent *objEv);
// Private variables
static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal;
AccelGyroSettingsData agcal;
// These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 };
static float mag_scale[3] = { 0, 0, 0 };
static float accel_bias[3] = { 0, 0, 0 };
static float accel_scale[3] = { 0, 0, 0 };
static float gyro_staticbias[3] = { 0, 0, 0 };
static float gyro_scale[3] = { 0, 0, 0 };
// temp coefficient to calculate gyro bias
static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false;
static float R[3][3] = {
{ 0 }
};
@ -113,12 +113,13 @@ int32_t SensorsInitialize(void)
MagSensorInitialize();
RevoCalibrationInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
rotate = 0;
RevoCalibrationConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -341,12 +342,24 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
}
// Scale the accels
float accels[3] = { (float)accel_accum[0] / accel_samples,
(float)accel_accum[1] / accel_samples,
(float)accel_accum[2] / accel_samples };
float accels_out[3] = { accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
accels[2] * accel_scaling * accel_scale[2] - accel_bias[2] };
float accels[3] = { (float)accel_accum[0] / accel_samples,
(float)accel_accum[1] / accel_samples,
(float)accel_accum[2] / accel_samples };
float accels_out[3] = { accels[0] * accel_scaling * agcal.accel_scale.X - agcal.accel_bias.X,
accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y,
accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z };
if (accel_temp_calibrated) {
float ctemp = accelSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
(accelSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
: accelSensorData.temperature);
accels_out[0] -= agcal.accel_temp_coeff.X * ctemp;
accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
}
if (rotate) {
rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0];
@ -360,12 +373,23 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
AccelSensorSet(&accelSensorData);
// Scale the gyros
float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
(float)gyro_accum[1] / gyro_samples,
(float)gyro_accum[2] / gyro_samples };
float gyros_out[3] = { gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0],
gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1],
gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2] };
float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
(float)gyro_accum[1] / gyro_samples,
(float)gyro_accum[2] / gyro_samples };
float gyros_out[3] = { gyros[0] * gyro_scaling * agcal.gyro_scale.X - agcal.gyro_bias.X,
gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y,
gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z };
if (gyro_temp_calibrated) {
float ctemp = gyroSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
(gyroSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
: gyroSensorData.temperature);
gyros_out[0] -= (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0];
@ -421,25 +445,20 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{
RevoCalibrationGet(&cal);
AccelGyroSettingsGet(&agcal);
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z;
mag_scale[0] = cal.mag_scale.X;
mag_scale[1] = cal.mag_scale.Y;
mag_scale[2] = cal.mag_scale.Z;
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z;
mag_scale[0] = cal.mag_scale.X;
mag_scale[1] = cal.mag_scale.Y;
mag_scale[2] = cal.mag_scale.Z;
accel_bias[0] = cal.accel_bias.X;
accel_bias[1] = cal.accel_bias.Y;
accel_bias[2] = cal.accel_bias.Z;
accel_scale[0] = cal.accel_scale.X;
accel_scale[1] = cal.accel_scale.Y;
accel_scale[2] = cal.accel_scale.Z;
gyro_staticbias[0] = cal.gyro_bias.X;
gyro_staticbias[1] = cal.gyro_bias.Y;
gyro_staticbias[2] = cal.gyro_bias.Z;
gyro_scale[0] = cal.gyro_scale.X;
gyro_scale[1] = cal.gyro_scale.Y;
gyro_scale[2] = cal.gyro_scale.Z;
accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
agcal.accel_temp_coeff.X > 1e-9f && agcal.accel_temp_coeff.Y > 1e-9f && agcal.accel_temp_coeff.Z > 1e-9f;
gyro_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
agcal.gyro_temp_coeff.X > 1e-9f && agcal.gyro_temp_coeff.Y > 1e-9f &&
agcal.gyro_temp_coeff.Z > 1e-9f && agcal.gyro_temp_coeff.Z2 > 1e-9f;
AttitudeSettingsData attitudeSettings;

89
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
@ -65,15 +66,20 @@
#include "relay_tuning.h"
// Private constants
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
#define MAX_QUEUE_SIZE 1
#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 + 4)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority
#define FAILSAFE_TIMEOUT_MS 30
// number of flight mode switch positions
@ -98,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;
@ -163,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();
@ -194,10 +203,13 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
static void stabilizationTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
uint32_t timeval = PIOS_DELAY_GetRaw();
PiOSDeltatimeConfig timeval;
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
float throttleDesired;
RateDesiredData rateDesired;
AttitudeStateData attitudeState;
GyroStateData gyroStateData;
@ -226,11 +238,10 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
continue;
}
dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
timeval = PIOS_DELAY_GetRaw();
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
ManualControlCommandThrottleGet(&throttleDesired);
AttitudeStateGet(&attitudeState);
GyroStateGet(&gyroStateData);
StabilizationBankGet(&stabBank);
@ -587,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
@ -640,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;
@ -648,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
@ -671,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;
@ -901,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
@ -928,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;

63
flight/modules/StateEstimation/filteraltitude.c
View File

@ -40,20 +40,22 @@
#define STACK_REQUIRED 128
#define DT_ALPHA 1e-3f
#define DT_ALPHA 1e-2f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_AVERAGE 1e-3f
// Private types
struct data {
float state[4]; // state = altitude,velocity,accel_offset,accel
float pos[3]; // position updates from other filters
float vel[3]; // position updates from other filters
float dTA;
float dTA2;
int32_t lastTime;
float accelLast;
float baroLast;
int32_t baroLastTime;
bool first_run;
float state[4]; // state = altitude,velocity,accel_offset,accel
float pos[3]; // position updates from other filters
float vel[3]; // position updates from other filters
PiOSDeltatimeConfig dt1config;
PiOSDeltatimeConfig dt2config;
float accelLast;
float baroLast;
bool first_run;
AltitudeFilterSettingsData settings;
};
@ -89,8 +91,8 @@ static int32_t init(stateFilter *self)
this->vel[0] = 0.0f;
this->vel[1] = 0.0f;
this->vel[2] = 0.0f;
this->dTA = -1.0f;
this->dTA2 = -1.0f;
PIOS_DELTATIME_Init(&this->dt1config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
PIOS_DELTATIME_Init(&this->dt2config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
this->baroLast = 0.0f;
this->accelLast = 0.0f;
this->first_run = 1;
@ -104,12 +106,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (this->first_run) {
// Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
this->lastTime = PIOS_DELAY_GetRaw();
}
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0;
this->baroLastTime = PIOS_DELAY_GetRaw();
this->first_run = 0;
}
} else {
// save existing position and velocity updates so GPS will still work
@ -141,22 +139,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// correct velocity and position state (integration)
// low pass for average dT, compensate timing jitter from scheduler
float dT = PIOS_DELAY_DiffuS(this->lastTime) / 1.0e6f;
this->lastTime = PIOS_DELAY_GetRaw();
if (dT < 0.001f) {
dT = 0.001f;
}
if (this->dTA < 0) {
this->dTA = dT;
} else {
this->dTA = this->dTA * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
}
//
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config);
float speedLast = this->state[1];
this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * this->dTA;
this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * dT;
this->accelLast = this->state[3] - this->state[2];
this->state[0] += 0.5f * (speedLast + this->state[1]) * this->dTA;
this->state[0] += 0.5f * (speedLast + this->state[1]) * dT;
state->pos[0] = this->pos[0];
@ -175,17 +165,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// correct the velocity state (low pass differentiation)
// low pass for average dT, compensate timing jitter from scheduler
float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f;
this->baroLastTime = PIOS_DELAY_GetRaw();
if (dT < 0.001f) {
dT = 0.001f;
}
if (this->dTA2 < 0) {
this->dTA2 = dT;
} else {
this->dTA2 = this->dTA2 * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
}
this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / this->dTA2;
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config);
this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
this->baroLast = state->baro[0];
state->pos[0] = this->pos[0];

34
flight/modules/StateEstimation/filterekf.c
View File

@ -45,6 +45,8 @@
#define STACK_REQUIRED 2048
#define DT_ALPHA 1e-3f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_INIT (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo)
#define IMPORT_SENSOR_IF_UPDATED(shortname, num) \
@ -66,10 +68,9 @@ struct data {
stateEstimation work;
uint32_t ins_last_time;
bool inited;
bool inited;
float dTa;
PiOSDeltatimeConfig dtconfig;
};
// Private variables
@ -154,11 +155,10 @@ static int32_t maininit(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->inited = false;
this->init_stage = 0;
this->work.updated = 0;
this->ins_last_time = PIOS_DELAY_GetRaw();
this->dTa = DT_INIT;
this->inited = false;
this->init_stage = 0;
this->work.updated = 0;
PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA);
EKFConfigurationGet(&this->ekfConfiguration);
int t;
@ -224,17 +224,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
return 0;
}
dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f;
this->ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if (dT > 0.01f) {
dT = 0.01f;
} else if (dT <= 0.001f) {
dT = 0.001f;
}
this->dTa = this->dTa * (1.0f - DT_ALPHA) + dT * DT_ALPHA; // low pass for average dT, compensate timing jitter from scheduler
dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig);
if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) {
// Don't initialize until all sensors are read
@ -300,7 +290,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// Run prediction a bit before any corrections
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
INSStatePrediction(gyros, this->work.accel, this->dTa);
INSStatePrediction(gyros, this->work.accel, dT);
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
@ -335,7 +325,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
// Advance the state estimate
INSStatePrediction(gyros, this->work.accel, this->dTa);
INSStatePrediction(gyros, this->work.accel, dT);
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
@ -355,7 +345,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
// Advance the covariance estimate
INSCovariancePrediction(this->dTa);
INSCovariancePrediction(dT);
if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS;

16
flight/modules/StateEstimation/stateestimation.c
View File

@ -31,6 +31,8 @@
#include "inc/stateestimation.h"
#include <callbackinfo.h>
#include <gyrosensor.h>
#include <accelsensor.h>
#include <magsensor.h>
@ -266,7 +268,7 @@ int32_t StateEstimationInitialize(void)
stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter));
stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, stack_required);
stateEstimationCallback = PIOS_CALLBACKSCHEDULER_Create(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required);
return 0;
}
@ -305,7 +307,7 @@ static void StateEstimationCb(void)
// after system startup, first few sensor readings might be messed up, delay until everything has settled
if (bootDelay) {
bootDelay--;
DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
return;
}
@ -388,7 +390,7 @@ static void StateEstimationCb(void)
// we are not done, re-dispatch self execution
runState = RUNSTATE_FILTER;
DelayedCallbackDispatch(stateEstimationCallback);
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
break;
case RUNSTATE_FILTER:
@ -405,7 +407,7 @@ static void StateEstimationCb(void)
if (!current) {
runState = RUNSTATE_SAVE;
}
DelayedCallbackDispatch(stateEstimationCallback);
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
break;
case RUNSTATE_SAVE:
@ -458,9 +460,9 @@ static void StateEstimationCb(void)
// we are done, re-schedule next self execution
runState = RUNSTATE_LOAD;
if (updatedSensors) {
DelayedCallbackDispatch(stateEstimationCallback);
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
} else {
DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
}
break;
}
@ -513,7 +515,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
updatedSensors |= SENSORUPDATES_airspeed;
}
DelayedCallbackDispatch(stateEstimationCallback);
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
}

48
flight/modules/System/systemmod.c
View File

@ -52,7 +52,7 @@
#include <i2cstats.h>
#include <taskinfo.h>
#include <watchdogstatus.h>
#include <taskinfo.h>
#include <callbackinfo.h>
#include <hwsettings.h>
#include <pios_flashfs.h>
#if defined(PIOS_INCLUDE_RFM22B)
@ -96,7 +96,7 @@ static uint32_t idleCounter;
static uint32_t idleCounterClear;
static xTaskHandle systemTaskHandle;
static xQueueHandle objectPersistenceQueue;
static bool stackOverflow;
static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
static bool mallocFailed;
static HwSettingsData bootHwSettings;
static struct PIOS_FLASHFS_Stats fsStats;
@ -105,6 +105,7 @@ static void objectUpdatedCb(UAVObjEvent *ev);
static void hwSettingsUpdatedCb(UAVObjEvent *ev);
#ifdef DIAG_TASKS
static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context);
static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context);
#endif
static void updateStats();
static void updateSystemAlarms();
@ -124,7 +125,7 @@ extern uintptr_t pios_user_fs_id;
int32_t SystemModStart(void)
{
// Initialize vars
stackOverflow = false;
stackOverflow = STACKOVERFLOW_NONE;
mallocFailed = false;
// Create system task
xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
@ -147,6 +148,7 @@ int32_t SystemModInitialize(void)
ObjectPersistenceInitialize();
#ifdef DIAG_TASKS
TaskInfoInitialize();
CallbackInfoInitialize();
#endif
#ifdef DIAG_I2C_WDG_STATS
I2CStatsInitialize();
@ -175,7 +177,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
MODULE_TASKCREATE_ALL;
/* start the delayed callback scheduler */
CallbackSchedulerStart();
PIOS_CALLBACKSCHEDULER_Start();
if (mallocFailed) {
/* We failed to malloc during task creation,
@ -204,6 +206,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
#ifdef DIAG_TASKS
TaskInfoData taskInfoData;
CallbackInfoData callbackInfoData;
#endif
// Main system loop
@ -223,6 +226,9 @@ static void systemTask(__attribute__((unused)) void *parameters)
// Update the task status object
PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData);
TaskInfoSet(&taskInfoData);
// Update the callback status object
PIOS_CALLBACKSCHEDULER_ForEachCallback(callbackSchedulerForEachCallback, &callbackInfoData);
CallbackInfoSet(&callbackInfoData);
#endif
// Flash the heartbeat LED
@ -440,7 +446,26 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_
((uint16_t *)&taskData->StackRemaining)[task_id] = task_info->stack_remaining;
((uint8_t *)&taskData->RunningTime)[task_id] = task_info->running_time_percentage;
}
#endif
static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context)
{
CallbackInfoData *callbackData = (CallbackInfoData *)context;
if (callback_id < 0) {
return;
}
// delayed callback scheduler reports callback stack overflows as remaininng: -1
if (callback_info->stack_remaining < 0 && stackOverflow == STACKOVERFLOW_NONE) {
stackOverflow = STACKOVERFLOW_WARNING;
}
// By convention, there is a direct mapping between (not negative) callback scheduler callback_id's and members
// of the CallbackInfoXXXXElem enums
PIOS_DEBUG_Assert(callback_id < CALLBACKINFO_RUNNING_NUMELEM);
((uint8_t *)&callbackData->Running)[callback_id] = callback_info->is_running;
((uint32_t *)&callbackData->RunningTime)[callback_id] = callback_info->running_time_count;
((int16_t *)&callbackData->StackRemaining)[callback_id] = callback_info->stack_remaining;
}
#endif /* ifdef DIAG_TASKS */
/**
* Called periodically to update the I2C statistics
@ -602,10 +627,15 @@ static void updateSystemAlarms()
}
// Check for stack overflow
if (stackOverflow) {
AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
} else {
switch (stackOverflow) {
case STACKOVERFLOW_NONE:
AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
break;
case STACKOVERFLOW_WARNING:
AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_WARNING);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
}
// Check for event errors
@ -650,7 +680,7 @@ void vApplicationIdleHook(void)
void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
__attribute__((unused)) signed portCHAR *pcTaskName)
{
stackOverflow = true;
stackOverflow = STACKOVERFLOW_CRITICAL;
#if DEBUG_STACK_OVERFLOW
static volatile bool wait_here = true;
while (wait_here) {

59
flight/modules/Telemetry/telemetry.c
View File

@ -45,7 +45,6 @@
#define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_RADRX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_TXPRI (tskIDLE_PRIORITY + 2)
#define REQ_TIMEOUT_MS 250
#define MAX_RETRIES 2
#define STATS_UPDATE_PERIOD_MS 4000
@ -59,8 +58,6 @@ static xQueueHandle queue;
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
static xQueueHandle priorityQueue;
static xTaskHandle telemetryTxPriTaskHandle;
static void telemetryTxPriTask(void *parameters);
#else
#define priorityQueue queue
#endif
@ -119,11 +116,6 @@ int32_t TelemetryStart(void)
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle);
#endif
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle);
#endif
return 0;
}
@ -276,7 +268,12 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
eventMask |= EV_LOGGING_MANUAL;
break;
}
UAVObjConnectQueue(obj, priorityQueue, eventMask);
// note that all setting objects have implicitly IsPriority=true
if (UAVObjIsPriority(obj)) {
UAVObjConnectQueue(obj, priorityQueue, eventMask);
} else {
UAVObjConnectQueue(obj, queue, eventMask);
}
}
/**
@ -374,32 +371,24 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
// Loop forever
while (1) {
// Wait for queue message
if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
}
}
/**
* Telemetry transmit task, high priority
*/
/**
* Tries to empty the high priority queue before handling any standard priority item
*/
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
static void telemetryTxPriTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
// Loop forever
while (1) {
// Loop forever
while (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
#endif
// Wait for queue message
if (xQueueReceive(priorityQueue, &ev, portMAX_DELAY) == pdTRUE) {
if (xQueueReceive(queue, &ev, 0) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
}
}
#endif
/**
* Telemetry receive task. Processes queue events and periodic updates.
@ -487,10 +476,13 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_UPDATED_PERIODIC;
ev.lowPriority = true;
ret = EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
if (ret == -1) {
ret = EventPeriodicQueueCreate(&ev, queue, updatePeriodMs);
ret = EventPeriodicQueueCreate(&ev, targetQueue, updatePeriodMs);
}
return ret;
}
@ -511,10 +503,13 @@ static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_LOGGING_PERIODIC;
ev.lowPriority = true;
ret = EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
if (ret == -1) {
ret = EventPeriodicQueueCreate(&ev, queue, updatePeriodMs);
ret = EventPeriodicQueueCreate(&ev, targetQueue, updatePeriodMs);
}
return ret;
}

1
flight/modules/TxPID/txpid.c
View File

@ -111,6 +111,7 @@ int32_t TxPIDInitialize(void)
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);

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;

172
flight/uavobjects/callbackscheduler.c → flight/pios/common/pios_callbackscheduler.c
View File

@ -1,7 +1,7 @@
/**
******************************************************************************
*
* @file callbackscheduler.c
* @file pios_callbackscheduler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Scheduler to run callback functions from a shared context with given priorities.
*
@ -24,13 +24,18 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <pios.h>
#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
#include <utlist.h>
#include <uavobjectmanager.h>
#include <taskinfo.h>
// Private constants
#define STACK_SIZE 128
#define MAX_SLEEP 1000
#define STACK_SAFETYCOUNT 16
#define STACK_SIZE (384 + STACK_SAFETYSIZE)
#define STACK_SAFETYSIZE 8
#define MAX_SLEEP 1000
// Private types
/**
@ -52,8 +57,15 @@ struct DelayedCallbackTaskStruct {
*/
struct DelayedCallbackInfoStruct {
DelayedCallback cb;
int16_t callbackID;
bool volatile waiting;
uint32_t volatile scheduletime;
uint32_t stackSize;
int32_t stackFree;
int32_t stackNotFree;
uint16_t stackSafetyCount;
uint16_t currentSafetyCount;
uint32_t runCount;
struct DelayedCallbackTaskStruct *task;
struct DelayedCallbackInfoStruct *next;
};
@ -73,7 +85,7 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa
* must be called before any other functions are called
* \return Success (0), failure (-1)
*/
int32_t CallbackSchedulerInitialize()
int32_t PIOS_CALLBACKSCHEDULER_Initialize()
{
// Initialize variables
schedulerTasks = NULL;
@ -99,7 +111,7 @@ int32_t CallbackSchedulerInitialize()
* they can be marked for later execution by executing the dispatch function.
* \return Success (0), failure (-1)
*/
int32_t CallbackSchedulerStart()
int32_t PIOS_CALLBACKSCHEDULER_Start()
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
@ -113,7 +125,7 @@ int32_t CallbackSchedulerStart()
xTaskCreate(
CallbackSchedulerTask,
cursor->name,
cursor->stackSize / 4,
1 + (cursor->stackSize / 4),
cursor,
cursor->priorityTask,
&cursor->callbackSchedulerTaskHandle
@ -143,7 +155,7 @@ int32_t CallbackSchedulerStart()
* UPDATEMODE_OVERRIDE: The callback will be rescheduled in any case, effectively overriding any previous schedule. (sooner+later=override)
* \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden
*/
int32_t DelayedCallbackSchedule(
int32_t PIOS_CALLBACKSCHEDULER_Schedule(
DelayedCallbackInfo *cbinfo,
int32_t milliseconds,
DelayedCallbackUpdateMode updatemode)
@ -191,7 +203,7 @@ int32_t DelayedCallbackSchedule(
* \param[in] cbinfo the callback handle
* \return Success (-1), failure (0)
*/
int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo)
int32_t PIOS_CALLBACKSCHEDULER_Dispatch(DelayedCallbackInfo *cbinfo)
{
PIOS_Assert(cbinfo);
@ -215,7 +227,7 @@ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo)
* Check the demo task for your port to find the syntax required.
* \return Success (-1), failure (0)
*/
int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken)
int32_t PIOS_CALLBACKSCHEDULER_DispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken)
{
PIOS_Assert(cbinfo);
@ -237,14 +249,18 @@ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigh
* \param[in] stacksize The stack requirements of the callback when called by the scheduler.
* \return CallbackInfo Pointer on success, NULL if failed.
*/
DelayedCallbackInfo *DelayedCallbackCreate(
DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create(
DelayedCallback cb,
DelayedCallbackPriority priority,
DelayedCallbackPriorityTask priorityTask,
int16_t callbackID,
uint32_t stacksize)
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// add callback schedulers own stack requirements
stacksize += STACK_SIZE;
// find appropriate scheduler task matching priorityTask
struct DelayedCallbackTaskStruct *task = NULL;
int t = 0;
@ -271,7 +287,7 @@ DelayedCallbackInfo *DelayedCallbackCreate(
task->name[0] = 'C';
task->name[1] = 'a' + t;
task->name[2] = 0;
task->stackSize = ((STACK_SIZE > stacksize) ? STACK_SIZE : stacksize);
task->stackSize = stacksize;
task->priorityTask = priorityTask;
task->next = NULL;
@ -285,13 +301,13 @@ DelayedCallbackInfo *DelayedCallbackCreate(
// add to list of scheduler tasks
LL_APPEND(schedulerTasks, task);
// Previously registered tasks are spawned when CallbackSchedulerStart() is called.
// Previously registered tasks are spawned when PIOS_CALLBACKSCHEDULER_Start() is called.
// Tasks registered afterwards need to spawn upon creation.
if (schedulerStarted) {
xTaskCreate(
CallbackSchedulerTask,
task->name,
task->stackSize / 4,
1 + (task->stackSize / 4),
task,
task->priorityTask,
&task->callbackSchedulerTaskHandle
@ -318,11 +334,18 @@ DelayedCallbackInfo *DelayedCallbackCreate(
xSemaphoreGiveRecursive(mutex);
return NULL; // error - not enough memory
}
info->next = NULL;
info->waiting = false;
info->scheduletime = 0;
info->task = task;
info->next = NULL;
info->waiting = false;
info->scheduletime = 0;
info->task = task;
info->cb = cb;
info->callbackID = callbackID;
info->runCount = 0;
info->stackSize = stacksize - STACK_SIZE;
info->stackNotFree = info->stackSize;
info->stackFree = 0;
info->stackSafetyCount = STACK_SAFETYCOUNT;
info->currentSafetyCount = 0;
// add to scheduling queue
LL_APPEND(task->callbackQueue[priority], info);
@ -332,6 +355,108 @@ DelayedCallbackInfo *DelayedCallbackCreate(
return info;
}
/**
* Iterator. Iterates over all callbacks and all scheduler tasks and retrieves information
*
* @param[in] callback Callback function to receive the data - will be called in same task context as the callerThe id of the task the task_info refers to.
* @param context Context information optionally provided to the callback.
*/
void PIOS_CALLBACKSCHEDULER_ForEachCallback(CallbackSchedulerCallbackInfoCallback callback, void *context)
{
if (!callback) {
return;
}
struct pios_callback_info info;
struct DelayedCallbackTaskStruct *task = NULL;
LL_FOREACH(schedulerTasks, task) {
int prio;
for (prio = 0; prio < (CALLBACK_PRIORITY_LOW + 1); prio++) {
struct DelayedCallbackInfoStruct *cbinfo;
LL_FOREACH(task->callbackQueue[prio], cbinfo) {
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
info.is_running = true;
info.stack_remaining = cbinfo->stackNotFree;
info.running_time_count = cbinfo->runCount;
xSemaphoreGiveRecursive(mutex);
callback(cbinfo->callbackID, &info, context);
}
}
}
}
/**
* Stack magic, find how much stack is being used without affecting performance
*/
static void markStack(DelayedCallbackInfo *current)
{
register int8_t t;
register int32_t halfWayMark;
volatile unsigned char *marker;
if (current->stackNotFree < 0) {
return;
}
// end of stack watermark
marker = (unsigned char *)(((size_t)&marker) - (size_t)current->stackSize);
for (t = -STACK_SAFETYSIZE; t < 0; t++) {
*(marker + t) = '#';
}
// shifted watermarks
halfWayMark = current->stackFree + (current->stackNotFree - current->stackFree) / 2;
marker = (unsigned char *)((size_t)marker + halfWayMark);
for (t = -STACK_SAFETYSIZE; t < 0; t++) {
*(marker + t) = '#';
}
}
static void checkStack(DelayedCallbackInfo *current)
{
register int8_t t;
register int32_t halfWayMark;
volatile unsigned char *marker;
if (current->stackNotFree < 0) {
return;
}
// end of stack watermark
marker = (unsigned char *)(((size_t)&marker) - (size_t)current->stackSize);
for (t = -STACK_SAFETYSIZE; t < 0; t++) {
if (*(marker + t) != '#') {
current->stackNotFree = -1; // stack overflow, disable all further checks
return;
}
}
// shifted watermarks
halfWayMark = current->stackFree + (current->stackNotFree - current->stackFree) / 2;
marker = (unsigned char *)((size_t)marker + halfWayMark);
for (t = -STACK_SAFETYSIZE; t < 0; t++) {
if (*(marker + t) != '#') {
current->stackNotFree = halfWayMark; // tainted mark, this place is definitely used stack
current->currentSafetyCount = 0;
if (current->stackNotFree <= current->stackFree) {
current->stackFree = 0; // if it was supposed to be free, restart search between here and bottom of stack
}
return;
}
}
if (current->currentSafetyCount < 0xffff) {
current->currentSafetyCount++; // mark has not been tainted, increase safety counter
}
if (current->currentSafetyCount >= current->stackSafetyCount) { // if the safety counter is above the limit, then
if (halfWayMark == current->stackFree) { // check if search already converged, if so increase the limit to find very rare stack usage incidents
current->stackSafetyCount = current->currentSafetyCount;
} else {
current->stackFree = halfWayMark; // otherwise just mark this position as free stack to narrow search
current->currentSafetyCount = 0;
}
}
}
/**
* Scheduler subtask
* \param[in] task The scheduler task in question
@ -384,7 +509,16 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa
current->scheduletime = 0; // any schedules are reset
current->waiting = false; // the flag is reset just before execution.
xSemaphoreGiveRecursive(mutex);
/* callback gets invoked here - check stack sizes */
markStack(current);
current->cb(); // call the callback
checkStack(current);
current->runCount++;
return 0;
}
xSemaphoreGiveRecursive(mutex);
@ -411,3 +545,5 @@ static void CallbackSchedulerTask(void *task)
}
}
}
#endif // ifdef PIOS_INCLUDE_CALLBACKSCHEDULER

82
flight/pios/common/pios_com.c
View File

@ -51,6 +51,7 @@ struct pios_com_dev {
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreHandle tx_sem;
xSemaphoreHandle rx_sem;
xSemaphoreHandle sendbuffer_sem;
#endif
bool has_rx;
@ -155,6 +156,9 @@ int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, ui
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev);
}
#if defined(PIOS_INCLUDE_FREERTOS)
com_dev->sendbuffer_sem = xSemaphoreCreateMutex();
#endif /* PIOS_INCLUDE_FREERTOS */
*com_id = (uint32_t)com_dev;
return 0;
@ -267,27 +271,11 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
return 0;
}
/**
* Sends a package over given port
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return number of bytes transmitted on success
*/
int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len)
static int32_t PIOS_COM_SendBufferNonBlockingInternal(struct pios_com_dev *com_dev, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id;
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev);
PIOS_Assert(com_dev->has_tx);
if (com_dev->driver->available && !com_dev->driver->available(com_dev->lower_id)) {
/*
* Underlying device is down/unconnected.
@ -314,10 +302,42 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u
fifoBuf_getUsed(&com_dev->tx));
}
}
return bytes_into_fifo;
}
/**
* Sends a package over given port
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return -3 another thread is already sending, caller should
* retry until com is available again
* \return number of bytes transmitted on success
*/
int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id;
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->sendbuffer_sem, 0) != pdTRUE) {
return -3;
}
#endif /* PIOS_INCLUDE_FREERTOS */
int32_t ret = PIOS_COM_SendBufferNonBlockingInternal(com_dev, buffer, len);
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
return ret;
}
/**
* Sends a package over given port
* (blocking function)
@ -325,6 +345,8 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if mutex can't be taken;
* \return -3 if data cannot be sent in the max allotted time of 5000msec
* \return number of bytes transmitted on success
*/
int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len)
@ -335,9 +357,12 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev->has_tx);
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->sendbuffer_sem, 5) != pdTRUE) {
return -2;
}
#endif /* PIOS_INCLUDE_FREERTOS */
uint32_t max_frag_len = fifoBuf_getSize(&com_dev->tx);
uint32_t bytes_to_send = len;
while (bytes_to_send) {
@ -348,13 +373,16 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
} else {
frag_size = bytes_to_send;
}
int32_t rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, frag_size);
int32_t rc = PIOS_COM_SendBufferNonBlockingInternal(com_dev, buffer, frag_size);
if (rc >= 0) {
bytes_to_send -= rc;
buffer += rc;
} else {
switch (rc) {
case -1:
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
/* Device is invalid, this will never work */
return -1;
@ -367,17 +395,23 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
}
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->tx_sem, 5000) != pdTRUE) {
xSemaphoreGive(com_dev->sendbuffer_sem);
return -3;
}
#endif
continue;
default:
/* Unhandled return code */
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
return rc;
}
}
}
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
return len;
}

64
flight/pios/common/pios_deltatime.c Normal file
View File

@ -0,0 +1,64 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_DELTATIME time measurement Functions
* @brief PiOS Delay functionality
* @{
*
* @file pios_deltatime.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Settings functions header
* @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 <pios.h>
void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha)
{
PIOS_Assert(config);
config->average = average;
config->min = min;
config->max = max;
config->alpha = alpha;
config->last = PIOS_DELAY_GetRaw();
};
float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config)
{
PIOS_Assert(config);
float dT = PIOS_DELAY_DiffuS(config->last) * 1.0e-6f;
config->last = PIOS_DELAY_GetRaw();
if (dT < config->min) {
dT = config->min;
}
if (dT > config->max) {
dT = config->max;
}
config->average = config->average * (1.0f - config->alpha) + dT * config->alpha;
return config->average;
}
/**
* @}
* @}
*/

42
flight/pios/common/pios_ms5611.c
View File

@ -32,6 +32,7 @@
#ifdef PIOS_INCLUDE_MS5611
#define POW2(x) (1 << x)
// TODO: Clean this up. Getting around old constant.
#define PIOS_MS5611_OVERSAMPLING oversampling
@ -166,7 +167,6 @@ uint32_t PIOS_MS5611_GetDelayUs()
/**
* Read the ADC conversion value (once ADC conversion has completed)
* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR
* \return 0 if successfully read the ADC, -1 if failed
*/
int32_t PIOS_MS5611_ReadADC(void)
@ -190,9 +190,12 @@ int32_t PIOS_MS5611_ReadADC(void)
}
RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] << 8);
Temperature = 2000l + ((deltaTemp * CalibData.C[5]) >> 23);
// Difference between actual and reference temperature
// dT = D2 - TREF = D2 - C5 * 2^8
deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] * POW2(8));
// Actual temperature (-40…85°C with 0.01°C resolution)
// TEMP = 20°C + dT * TEMPSENS = 2000 + dT * C6 / 2^23
Temperature = 2000l + ((deltaTemp * CalibData.C[5]) / POW2(23));
} else {
int64_t Offset;
int64_t Sens;
@ -206,14 +209,22 @@ int32_t PIOS_MS5611_ReadADC(void)
}
// check if temperature is less than 20°C
if (Temperature < 2000) {
Offset2 = 5 * (Temperature - 2000) >> 1;
Sens2 = Offset2 >> 1;
// Apply compensation
// T2 = dT^2 / 2^31
// OFF2 = 5 ⋅ (TEMP – 2000)^2/2
// SENS2 = 5 ⋅ (TEMP – 2000)^2/2^2
int64_t tcorr = (Temperature - 2000) * (Temperature - 2000);
Offset2 = (5 * tcorr) / 2;
Sens2 = (5 * tcorr) / 4;
compensation_t2 = (deltaTemp * deltaTemp) >> 31;
// Apply the "Very low temperature compensation" when temp is less than -15°C
if (Temperature < -1500) {
int64_t tcorr = (Temperature + 1500) * (Temperature + 1500);
Offset2 += 7 * tcorr;
Sens2 += (11 * tcorr) >> 1;
// OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2
// SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2
int64_t tcorr2 = (Temperature + 1500) * (Temperature + 1500);
Offset2 += 7 * tcorr2;
Sens2 += (11 * tcorr2) / 2;
}
} else {
compensation_t2 = 0;
@ -221,10 +232,15 @@ int32_t PIOS_MS5611_ReadADC(void)
Sens2 = 0;
}
RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]);
Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2;
Sens = ((int64_t)CalibData.C[0]) << 15;
Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8) - Sens2;
Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15;
// Offset at actual temperature
// OFF = OFFT1 + TCO * dT = C2 * 2^16 + (C4 * dT) / 2^7
Offset = ((int64_t)CalibData.C[1]) * POW2(16) + (((int64_t)CalibData.C[3]) * deltaTemp) / POW2(7) - Offset2;
// Sensitivity at actual temperature
// SENS = SENST1 + TCS * dT = C1 * 2^15 + (C3 * dT) / 2^8
Sens = ((int64_t)CalibData.C[0]) * POW2(15) + (((int64_t)CalibData.C[2]) * deltaTemp) / POW2(8) - Sens2;
// Temperature compensated pressure (10…1200mbar with 0.01mbar resolution)
// P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15
Pressure = (((((int64_t)RawPressure) * Sens) / POW2(21)) - Offset) / POW2(15);
}
return 0;
}

2
flight/pios/common/pios_rfm22b.c
View File

@ -64,7 +64,7 @@
/* Local Defines */
#define STACK_SIZE_BYTES 200
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // flight control relevant device driver (ppm link)
#define ISR_TIMEOUT 1 // ms
#define EVENT_QUEUE_SIZE 5
#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600

58
flight/uavobjects/inc/callbackscheduler.h → flight/pios/inc/pios_callbackscheduler.h
View File

@ -1,9 +1,9 @@
/**
******************************************************************************
*
* @file callbackscheduler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include files of the uavobjectlist library
* @file pios_callbackscheduler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Include files of the PIOS_CALLBACKSCHEDULER
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -23,8 +23,8 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef CALLBACKSCHEDULER_H
#define CALLBACKSCHEDULER_H
#ifndef PIOS_CALLBACKSCHEDULER_H
#define PIOS_CALLBACKSCHEDULER_H
// Public types
typedef enum {
@ -48,7 +48,7 @@ typedef enum {
// ...ABcABdABxABcABdAByABcABdABxABcABdABy...
// However if only the 3 callbacks, A, c and x want to execute, you will get:
// ...AcAxAcAxAcAxAcAxAcAxAcAxAcAxAcAxAcAx...
// And if onlz A and y need execution it will be:
// And if only A and y need execution it will be:
// ...AyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAy...
// despite their different priority they would get treated equally in this case.
//
@ -113,7 +113,7 @@ typedef struct DelayedCallbackInfoStruct DelayedCallbackInfo;
* must be called before any other functions are called
* \return Success (0), failure (-1)
*/
int32_t CallbackSchedulerInitialize();
int32_t PIOS_CALLBACKSCHEDULER_Initialize();
/**
* Start all scheduler tasks
@ -125,7 +125,7 @@ int32_t CallbackSchedulerInitialize();
* they can be marked for later execution by executing the dispatch function.
* \return Success (0), failure (-1)
*/
int32_t CallbackSchedulerStart();
int32_t PIOS_CALLBACKSCHEDULER_Start();
/**
* Register a new callback to be called by a delayed callback scheduler task.
@ -135,12 +135,14 @@ int32_t CallbackSchedulerStart();
* \param[in] priority Priority of the callback compared to other callbacks scheduled by the same delayed callback scheduler task.
* \param[in] priorityTask Task priority of the scheduler task. One scheduler task will be spawned for each distinct value specified, further callbacks created with the same priorityTask will all be handled by the same delayed callback scheduler task and scheduled according to their individual callback priorities
* \param[in] stacksize The stack requirements of the callback when called by the scheduler.
* \param[in] callbackID - CallbackInfoRunningElem from CallbackInfo UAVObject, unique identified to collect stats for the callback, -1 to ignore!
* \return CallbackInfo Pointer on success, NULL if failed.
*/
DelayedCallbackInfo *DelayedCallbackCreate(
DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create(
DelayedCallback cb,
DelayedCallbackPriority priority,
DelayedCallbackPriorityTask priorityTask,
int16_t callbackID,
uint32_t stacksize);
/**
@ -155,7 +157,7 @@ DelayedCallbackInfo *DelayedCallbackCreate(
* UPDATEMODE_OVERRIDE: The callback will be rescheduled in any case, effectively overriding any previous schedule. (sooner+later=override)
* \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden
*/
int32_t DelayedCallbackSchedule(
int32_t PIOS_CALLBACKSCHEDULER_Schedule(
DelayedCallbackInfo *cbinfo,
int32_t milliseconds,
DelayedCallbackUpdateMode updatemode);
@ -166,7 +168,7 @@ int32_t DelayedCallbackSchedule(
* \param[in] *cbinfo the callback handle
* \return Success (-1), failure (0)
*/
int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo);
int32_t PIOS_CALLBACKSCHEDULER_Dispatch(DelayedCallbackInfo *cbinfo);
/**
* Dispatch an event by invoking the supplied callback. The function
@ -182,6 +184,36 @@ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo);
* Check the demo task for your port to find the syntax required.
* \return Success (-1), failure (0)
*/
int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken);
int32_t PIOS_CALLBACKSCHEDULER_DispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken);
#endif // CALLBACKSCHEDULER_H
/**
* Information about a running callback that has been registered
* via a call to PIOS_CALLBACKSCHEDULER_Create().
*/
struct pios_callback_info {
/** Remaining task stack in bytes -1 for detected stack overflow. */
int32_t stack_remaining;
/** Flag indicating whether or not the task is running. */
bool is_running;
/** Count of executions of the callback since system start */
uint32_t running_time_count;
};
/**
* Iterator callback, called for each monitored callback by PIOS_CALLBACKSCHEDULER_ForEachCallback().
*
* @param task_id The id of the task the task_info refers to.
* @param task_info Information about the task identified by task_id.
* @param context Context information optionally provided by the caller to PIOS_TASK_MONITOR_TasksIterate()
*/
typedef void (*CallbackSchedulerCallbackInfoCallback)(int16_t task_id, const struct pios_callback_info *callback_info, void *context);
/**
* Iterator. Iterates over all callbacks and all scheduler tasks and retrieves information
*
* @param[in] callback Callback function to receive the data - will be called in same task context as the callerThe id of the task the task_info refers to.
* @param context Context information optionally provided to the callback.
*/
void PIOS_CALLBACKSCHEDULER_ForEachCallback(CallbackSchedulerCallbackInfoCallback callback, void *context);
#endif // PIOS_CALLBACKSCHEDULER_H

53
flight/pios/inc/pios_deltatime.h Normal file
View File

@ -0,0 +1,53 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_DELTATIME time measurement Functions
* @brief PiOS Delay functionality
* @{
*
* @file pios_deltatime.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Settings functions header
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_DELTATIME_H
#define PIOS_DELTATIME_H
struct PiOSDeltatimeConfigStruct {
uint32_t last;
float average;
float min;
float max;
float alpha;
};
typedef struct PiOSDeltatimeConfigStruct PiOSDeltatimeConfig;
/* Public Functions */
void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha);
float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config);
#endif /* PIOS_DELTATIME_H */
/**
* @}
* @}
*/

9
flight/pios/pios.h
View File

@ -93,6 +93,14 @@
#include <pios_task_monitor.h>
#endif
/* PIOS CallbackScheduler */
#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
#ifndef PIOS_INCLUDE_FREERTOS
#error PiOS CallbackScheduler requires PIOS_INCLUDE_FREERTOS to be defined
#endif
#include <pios_callbackscheduler.h>
#endif
/* PIOS bootloader helper */
#ifdef PIOS_INCLUDE_BL_HELPER
/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */
@ -102,6 +110,7 @@
/* PIOS system functions */
#ifdef PIOS_INCLUDE_DELAY
#include <pios_delay.h>
#include <pios_deltatime.h>
#endif
#ifdef PIOS_INCLUDE_INITCALL

9
flight/pios/pios_sim_posix.h
View File

@ -47,6 +47,14 @@
#include <pios_task_monitor.h>
#endif
/* PIOS CallbackScheduler */
#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
#ifndef PIOS_INCLUDE_FREERTOS
#error PiOS CallbackScheduler requires PIOS_INCLUDE_FREERTOS to be defined
#endif
#include <pios_callbackscheduler.h>
#endif
/* C Lib Includes */
#include <stdio.h>
#include <stdlib.h>
@ -79,6 +87,7 @@ extern void PIOS_LED_Init(void);
#include <pios_wdg.h>
#include <pios_debug.h>
#include <pios_debuglog.h>
#include <pios_deltatime.h>
#include <pios_crc.h>
#include <pios_rcvr.h>
#include <pios_flash.h>

5
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
@ -70,7 +71,6 @@ ifndef TESTAPP
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/callbackscheduler.c
## UAVObjects
SRC += $(OPUAVSYNTHDIR)/accessorydesired.c
@ -92,12 +92,14 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
SRC += $(OPUAVSYNTHDIR)/accelstate.c
SRC += $(OPUAVSYNTHDIR)/accelgyrosettings.c
SRC += $(OPUAVSYNTHDIR)/gyrostate.c
SRC += $(OPUAVSYNTHDIR)/attitudestate.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
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
@ -112,6 +114,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/relaytuningsettings.c
SRC += $(OPUAVSYNTHDIR)/relaytuning.c
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/ratedesired.c
SRC += $(OPUAVSYNTHDIR)/barosensor.c

1
flight/targets/boards/coptercontrol/firmware/inc/openpilot.h
View File

@ -36,7 +36,6 @@
#include <utlist.h>
#include <uavobjectmanager.h>
#include <eventdispatcher.h>
#include <callbackscheduler.h>
#include <uavtalk.h>
#include "alarms.h"

5
flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
View File

@ -43,6 +43,10 @@
/* PIOS FreeRTOS support */
#define PIOS_INCLUDE_FREERTOS
/* PIOS CallbackScheduler support */
#define PIOS_INCLUDE_CALLBACKSCHEDULER
/* PIOS bootloader helper */
#define PIOS_INCLUDE_BL_HELPER
/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */
@ -160,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/coptercontrol/firmware/inc/pios_config_posix.h
View File

@ -34,6 +34,7 @@
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_CALLBACKSCHEDULER
#define PIOS_INCLUDE_TASK_MONITOR
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_UDP

2
flight/targets/boards/coptercontrol/firmware/pios_board.c
View File

@ -198,7 +198,7 @@ void PIOS_Board_Init(void)
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();

2
flight/targets/boards/coptercontrol/firmware/pios_board_posix.c
View File

@ -39,7 +39,7 @@ void PIOS_Board_Init(void)
PIOS_DELAY_Init();
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();

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

@ -46,7 +46,6 @@ ifndef TESTAPP
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/callbackscheduler.c
## UAVObjects
SRC += $(OPUAVSYNTHDIR)/oplinkstatus.c

1
flight/targets/boards/oplinkmini/firmware/inc/openpilot.h
View File

@ -37,7 +37,6 @@
#include <utlist.h>
#include <uavobjectmanager.h>
#include <eventdispatcher.h>
#include <callbackscheduler.h>
#include <uavtalk.h>
#include "alarms.h"

3
flight/targets/boards/oplinkmini/firmware/inc/pios_config.h
View File

@ -43,6 +43,9 @@
/* PIOS FreeRTOS support */
#define PIOS_INCLUDE_FREERTOS
/* PIOS CallbackScheduler support */
#define PIOS_INCLUDE_CALLBACKSCHEDULER
/* PIOS bootloader helper */
#define PIOS_INCLUDE_BL_HELPER
/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

1
flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h
View File

@ -34,6 +34,7 @@
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_CALLBACKSCHEDULER
#define PIOS_INCLUDE_TASK_MONITOR
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_UDP

2
flight/targets/boards/oplinkmini/firmware/pios_board.c
View File

@ -106,7 +106,7 @@ void PIOS_Board_Init(void)
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();

2
flight/targets/boards/oplinkmini/firmware/pios_board_posix.c
View File

@ -39,7 +39,7 @@ void PIOS_Board_Init(void)
PIOS_DELAY_Init();
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();

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

@ -55,7 +55,6 @@ ifndef TESTAPP
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/callbackscheduler.c
## OSD fonts
SRC += $(OPSYSTEM)/fonts.c
@ -84,6 +83,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/homelocation.c
SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c

1
flight/targets/boards/osd/firmware/inc/openpilot.h
View File

@ -36,7 +36,6 @@
#include <utlist.h>
#include <uavobjectmanager.h>
#include <eventdispatcher.h>
#include <callbackscheduler.h>
#include <uavtalk.h>
#include "alarms.h"

3
flight/targets/boards/osd/firmware/inc/pios_config.h
View File

@ -43,6 +43,9 @@
/* PIOS FreeRTOS support */
#define PIOS_INCLUDE_FREERTOS
/* PIOS CallbackScheduler support */
#define PIOS_INCLUDE_CALLBACKSCHEDULER
/* PIOS bootloader helper */
#define PIOS_INCLUDE_BL_HELPER
/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

2
flight/targets/boards/osd/firmware/pios_board.c
View File

@ -178,7 +178,7 @@ void PIOS_Board_Init(void)
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();

2
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
@ -73,7 +74,6 @@ ifndef TESTAPP
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/callbackscheduler.c
#ifeq ($(DEBUG), YES)
SRC += $(OPSYSTEM)/dcc_stdio.c

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

@ -141,7 +141,6 @@ SRC += $(OPSYSTEM)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/callbackscheduler.c
SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
else
## TESTCODE

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

@ -19,6 +19,7 @@
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
@ -60,6 +61,7 @@ UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += flightmodesettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel
@ -90,6 +92,7 @@ UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
UAVOBJSRCFILENAMES += taskinfo
UAVOBJSRCFILENAMES += callbackinfo
UAVOBJSRCFILENAMES += velocitystate
UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus

1
flight/targets/boards/revolution/firmware/inc/openpilot.h
View File

@ -37,7 +37,6 @@
#include <utlist.h>
#include <uavobjectmanager.h>
#include <eventdispatcher.h>
#include <callbackscheduler.h>
#include <uavtalk.h>
#include "alarms.h"

3
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -43,6 +43,9 @@
/* PIOS FreeRTOS support */
#define PIOS_INCLUDE_FREERTOS
/* PIOS Callback Scheduler support */
#define PIOS_INCLUDE_CALLBACKSCHEDULER
/* PIOS bootloader helper */
#define PIOS_INCLUDE_BL_HELPER
/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

1
flight/targets/boards/revolution/firmware/inc/pios_config_sim.h
View File

@ -35,6 +35,7 @@
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SDCARD
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_CALLBACKSCHEDULER
#define PIOS_INCLUDE_TASK_MONITOR
#define PIOS_INCLUDE_COM
// #define PIOS_INCLUDE_GPS

2
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -404,7 +404,7 @@ void PIOS_Board_Init(void)
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();

2
flight/targets/boards/revolution/firmware/pios_board_sim.c
View File

@ -132,7 +132,7 @@ void PIOS_Board_Init(void)
PIOS_DELAY_Init();
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();

2
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
@ -71,7 +72,6 @@ ifndef TESTAPP
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/callbackscheduler.c
#ifeq ($(DEBUG), YES)
SRC += $(OPSYSTEM)/dcc_stdio.c

1
flight/targets/boards/revoproto/firmware/Makefile.osx
View File

@ -145,7 +145,6 @@ SRC += $(OPSYSTEM)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/callbackscheduler.c
SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c

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

@ -24,6 +24,7 @@
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired
@ -61,6 +62,7 @@ UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += flightmodesettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel
@ -90,6 +92,7 @@ UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
UAVOBJSRCFILENAMES += taskinfo
UAVOBJSRCFILENAMES += callbackinfo
UAVOBJSRCFILENAMES += velocitystate
UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus

1
flight/targets/boards/revoproto/firmware/inc/openpilot.h
View File

@ -37,7 +37,6 @@
#include <utlist.h>
#include <uavobjectmanager.h>
#include <eventdispatcher.h>
#include <callbackscheduler.h>
#include <uavtalk.h>
#include "alarms.h"

3
flight/targets/boards/revoproto/firmware/inc/pios_config.h
View File

@ -43,6 +43,9 @@
/* PIOS FreeRTOS support */
#define PIOS_INCLUDE_FREERTOS
/* PIOS Callback Scheduler support */
#define PIOS_INCLUDE_CALLBACKSCHEDULER
/* PIOS bootloader helper */
#define PIOS_INCLUDE_BL_HELPER
/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

1
flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h
View File

@ -35,6 +35,7 @@
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SDCARD
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_CALLBACKSCHEDULER
#define PIOS_INCLUDE_TASK_MONITOR
#define PIOS_INCLUDE_COM
// #define PIOS_INCLUDE_GPS

2
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -444,7 +444,7 @@ void PIOS_Board_Init(void)
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();

2
flight/targets/boards/revoproto/firmware/pios_board_sim.c
View File

@ -132,7 +132,7 @@ void PIOS_Board_Init(void)
PIOS_DELAY_Init();
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();

3
flight/targets/boards/simposix/firmware/Makefile
View File

@ -85,7 +85,6 @@ SRC += $(FLIGHTLIB)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/callbackscheduler.c
SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
@ -101,6 +100,8 @@ SRC += $(MATHLIB)/pid.c
SRC += $(PIOSCORECOMMON)/pios_task_monitor.c
SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c
SRC += $(PIOSCORECOMMON)/pios_debuglog.c
SRC += $(PIOSCORECOMMON)/pios_callbackscheduler.c
SRC += $(PIOSCORECOMMON)/pios_deltatime.c
## PIOS Hardware
include $(PIOS)/posix/library.mk

2
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
@ -90,6 +91,7 @@ UAVOBJSRCFILENAMES += systemalarms
UAVOBJSRCFILENAMES += systemsettings
UAVOBJSRCFILENAMES += systemstats
UAVOBJSRCFILENAMES += taskinfo
UAVOBJSRCFILENAMES += callbackinfo
UAVOBJSRCFILENAMES += velocitystate
UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus

1
flight/targets/boards/simposix/firmware/inc/openpilot.h
View File

@ -37,7 +37,6 @@
#include <utlist.h>
#include <uavobjectmanager.h>
#include <eventdispatcher.h>
#include <callbackscheduler.h>
#include <uavtalk.h>
#include "alarms.h"

1
flight/targets/boards/simposix/firmware/inc/pios_config.h
View File

@ -36,6 +36,7 @@
/* Major features */
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_CALLBACKSCHEDULER
#define PIOS_INCLUDE_BL_HELPER
/* Enable/Disable PiOS Modules */

2
flight/targets/boards/simposix/firmware/pios_board.c
View File

@ -132,7 +132,7 @@ void PIOS_Board_Init(void)
}
/* Initialize the delayed callback library */
CallbackSchedulerInitialize();
PIOS_CALLBACKSCHEDULER_Initialize();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();

13
flight/uavobjects/eventdispatcher.c
View File

@ -26,7 +26,7 @@
#include <openpilot.h>
#include <taskinfo.h>
#include <callbackinfo.h>
// Private constants
#if defined(PIOS_EVENTDISAPTCHER_QUEUE)
@ -55,6 +55,7 @@ typedef struct {
UAVObjEvent ev; /** The actual event */
UAVObjEventCallback cb; /** The callback function, or zero if none */
xQueueHandle queue; /** The queue or zero if none */
bool lowpriority; /** set to true for telemetry and other low priority stuffs, prevent raising warning */
} EventCallbackInfo;
/**
@ -103,8 +104,8 @@ int32_t EventDispatcherInitialize()
mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo));
// Create callback
eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE * 4);
DelayedCallbackDispatch(eventSchedulerCallback);
eventSchedulerCallback = PIOS_CALLBACKSCHEDULER_Create(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_EVENTDISPATCHER, STACK_SIZE * 4);
PIOS_CALLBACKSCHEDULER_Dispatch(eventSchedulerCallback);
// Done
return 0;
@ -148,7 +149,7 @@ int32_t EventCallbackDispatch(UAVObjEvent *ev, UAVObjEventCallback cb)
evInfo.queue = 0;
// Push to queue
int32_t result = xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full
DelayedCallbackDispatch(eventSchedulerCallback);
PIOS_CALLBACKSCHEDULER_Dispatch(eventSchedulerCallback);
return result;
}
@ -306,7 +307,7 @@ static void eventTask()
timeToNextUpdateMs = processPeriodicUpdates();
}
DelayedCallbackSchedule(eventSchedulerCallback, timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS), CALLBACK_UPDATEMODE_SOONER);
PIOS_CALLBACKSCHEDULER_Schedule(eventSchedulerCallback, timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS), CALLBACK_UPDATEMODE_SOONER);
}
/**
@ -341,7 +342,7 @@ static int32_t processPeriodicUpdates()
}
// Push event to queue, if one
if (objEntry->evInfo.queue != 0) {
if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE) { // do not block if queue is full
if (xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE && !objEntry->evInfo.ev.lowPriority) { // do not block if queue is full
if (objEntry->evInfo.ev.obj != NULL) {
mStats.lastErrorID = UAVObjGetID(objEntry->evInfo.ev.obj);
}

1
flight/uavobjects/inc/uavobject.h.template
View File

@ -44,6 +44,7 @@
#define $(NAMEUC)_OBJID $(OBJIDHEX)
#define $(NAMEUC)_ISSINGLEINST $(ISSINGLEINST)
#define $(NAMEUC)_ISSETTINGS $(ISSETTINGS)
#define $(NAMEUC)_ISPRIORITY $(ISPRIORITY)
#define $(NAMEUC)_NUMBYTES sizeof($(NAME)Data)
/* Generic interface functions */

5
flight/uavobjects/inc/uavobjectmanager.h
View File

@ -120,6 +120,7 @@ typedef struct {
UAVObjHandle obj;
uint16_t instId;
UAVObjEventType event;
bool lowPriority; /* if true prevents raising warnings */
} UAVObjEvent;
/**
@ -147,8 +148,7 @@ typedef struct {
int32_t UAVObjInitialize();
void UAVObjGetStats(UAVObjStats *statsOut);
void UAVObjClearStats();
UAVObjHandle UAVObjRegister(uint32_t id,
int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb);
UAVObjHandle UAVObjRegister(uint32_t id, bool isSingleInstance, bool isSettings, bool isPriority, uint32_t num_bytes, UAVObjInitializeCallback initCb);
UAVObjHandle UAVObjGetByID(uint32_t id);
uint32_t UAVObjGetID(UAVObjHandle obj);
uint32_t UAVObjGetNumBytes(UAVObjHandle obj);
@ -158,6 +158,7 @@ uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, UAVObjInitializeCallback
bool UAVObjIsSingleInstance(UAVObjHandle obj);
bool UAVObjIsMetaobject(UAVObjHandle obj);
bool UAVObjIsSettings(UAVObjHandle obj);
bool UAVObjIsPriority(UAVObjHandle obj);
int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn);
int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut);
uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc);

2
flight/uavobjects/uavobject.c.template
View File

@ -65,7 +65,7 @@ int32_t $(NAME)Initialize(void)
// Register object with the object manager
handle = UAVObjRegister($(NAMEUC)_OBJID,
$(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults);
$(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_ISPRIORITY, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults);
// Done
return handle ? 0 : -1;

25
flight/uavobjects/uavobjectmanager.c
View File

@ -108,6 +108,7 @@ struct UAVOBase {
bool isMeta : 1;
bool isSingle : 1;
bool isSettings : 1;
bool isPriority : 1;
} flags;
} __attribute__((packed));
@ -339,7 +340,7 @@ static struct UAVOData *UAVObjAllocMulti(uint32_t num_bytes)
* \return
*/
UAVObjHandle UAVObjRegister(uint32_t id,
int32_t isSingleInstance, int32_t isSettings,
bool isSingleInstance, bool isSettings, bool isPriority,
uint32_t num_bytes,
UAVObjInitializeCallback initCb)
{
@ -368,8 +369,11 @@ UAVObjHandle UAVObjRegister(uint32_t id,
uavo_data->instance_size = num_bytes;
if (isSettings) {
uavo_data->base.flags.isSettings = true;
// settings defaults to being sent with priority
uavo_data->base.flags.isPriority = true;
} else {
uavo_data->base.flags.isPriority = isPriority;
}
/* Initialize the embedded meta UAVO */
UAVObjInitMetaData(&uavo_data->metaObj);
@ -605,6 +609,22 @@ bool UAVObjIsSettings(UAVObjHandle obj_handle)
return uavo_base->flags.isSettings;
}
/**
* Is this a prioritized object?
* \param[in] obj The object handle
* \return True (1) if this is a prioritized object
*/
bool UAVObjIsPriority(UAVObjHandle obj_handle)
{
PIOS_Assert(obj_handle);
/* Recover the common object header */
struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
return uavo_base->flags.isPriority;
}
/**
* Unpack an object from a byte array
* \param[in] obj The object handle
@ -1719,6 +1739,7 @@ static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType
.obj = (UAVObjHandle)obj,
.event = triggered_event,
.instId = instId,
.lowPriority = false,
};
// Go through each object and push the event message in the queue (if event is activated for the queue)

130
ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml
View File

@ -2252,49 +2252,29 @@
<configurationStreamVersion>1000</configurationStreamVersion>
<dataSize>240</dataSize>
<plotCurve0>
<color>4294945280</color>
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-System</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>1.02360527502876e-306</yMaximum>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>6.92921153577169e-310</yMinimum>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve0>
<plotCurve1>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Actuator</uavField>
<uavField>StackRemaining-CallbackScheduler0</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve1>
<plotCurve10>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Guidance</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>6.92921442421083e-310</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>-1.29073709209104e-231</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve10>
<plotCurve11>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Watchdog</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>6.92920724098472e-310</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>3.91299991506267e-321</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve11>
<plotCurve2>
<color>4294945280</color>
<color>4278190335</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-TelemetryTx</uavField>
<uavField>StackRemaining-CallbackScheduler1</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2302,43 +2282,43 @@
<yScalePower>0</yScalePower>
</plotCurve2>
<plotCurve3>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-TelemetryTxPri</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>1.72723371101889e-77</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>1.72723371101889e-77</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve3>
<plotCurve4>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-TelemetryRx</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>6.92921438535612e-310</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>1.72723371101889e-77</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve4>
<plotCurve5>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-GPS</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>1.72723371101889e-77</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>1.03979250816176e-312</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve5>
<plotCurve6>
<color>4294945280</color>
<color>4294967040</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-ManualControl</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>6.92921438705062e-310</yMaximum>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>6.92921152810932e-310</yMinimum>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve3>
<plotCurve4>
<color>4278255615</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Stabilization</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve4>
<plotCurve5>
<color>4294923775</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Actuator</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve5>
<plotCurve6>
<color>4289331327</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Sensors</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve6>
<plotCurve7>
@ -2346,32 +2326,42 @@
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Altitude</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>7.4109846876187e-323</yMaximum>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>1.72723371101889e-77</yMinimum>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve7>
<plotCurve8>
<color>4294945280</color>
<color>4283760767</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-AHRSComms</uavField>
<uavField>StackRemaining-TelemetryTx</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>5.43472210425371e-323</yMaximum>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>1.72723371101889e-77</yMinimum>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve8>
<plotCurve9>
<color>4294945280</color>
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Stabilization</uavField>
<uavField>StackRemaining-TelemetryRx</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>1.72723371101889e-77</yMaximum>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>1.72723371101889e-77</yMinimum>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve9>
<plotCurveCount>12</plotCurveCount>
<plotCurve10>
<color>4283782527</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-RadioRx</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve10>
<plotCurveCount>11</plotCurveCount>
<plotType>1</plotType>
<refreshInterval>1000</refreshInterval>
</data>

62
ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml
View File

@ -2285,7 +2285,7 @@
<configurationStreamVersion>1000</configurationStreamVersion>
<dataSize>240</dataSize>
<plotCurve0>
<color>4294945280</color>
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-System</uavField>
<uavObject>TaskInfo</uavObject>
@ -2297,37 +2297,17 @@
<plotCurve1>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Actuator</uavField>
<uavField>StackRemaining-CallbackScheduler0</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve1>
<plotCurve10>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Guidance</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve10>
<plotCurve11>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Watchdog</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve11>
<plotCurve2>
<color>4294945280</color>
<color>4278190335</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-TelemetryTx</uavField>
<uavField>StackRemaining-CallbackScheduler1</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2335,9 +2315,9 @@
<yScalePower>0</yScalePower>
</plotCurve2>
<plotCurve3>
<color>4294945280</color>
<color>4294967040</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-TelemetryTxPri</uavField>
<uavField>StackRemaining-ManualControl</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2345,9 +2325,9 @@
<yScalePower>0</yScalePower>
</plotCurve3>
<plotCurve4>
<color>4294945280</color>
<color>4278255615</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-TelemetryRx</uavField>
<uavField>StackRemaining-Stabilization</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2355,9 +2335,9 @@
<yScalePower>0</yScalePower>
</plotCurve4>
<plotCurve5>
<color>4294945280</color>
<color>4294923775</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-GPS</uavField>
<uavField>StackRemaining-Actuator</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2365,9 +2345,9 @@
<yScalePower>0</yScalePower>
</plotCurve5>
<plotCurve6>
<color>4294945280</color>
<color>4289331327</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-ManualControl</uavField>
<uavField>StackRemaining-Sensors</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2385,9 +2365,9 @@
<yScalePower>0</yScalePower>
</plotCurve7>
<plotCurve8>
<color>4294945280</color>
<color>4283760767</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-AHRSComms</uavField>
<uavField>StackRemaining-TelemetryTx</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2395,15 +2375,25 @@
<yScalePower>0</yScalePower>
</plotCurve8>
<plotCurve9>
<color>4294945280</color>
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Stabilization</uavField>
<uavField>StackRemaining-TelemetryRx</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve9>
<plotCurve10>
<color>4283782527</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-RadioRx</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve10>
<plotCurveCount>12</plotCurveCount>
<plotType>1</plotType>
<refreshInterval>1000</refreshInterval>

64
ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml
View File

@ -2260,7 +2260,7 @@
<configurationStreamVersion>1000</configurationStreamVersion>
<dataSize>240</dataSize>
<plotCurve0>
<color>4294945280</color>
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-System</uavField>
<uavObject>TaskInfo</uavObject>
@ -2272,37 +2272,17 @@
<plotCurve1>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Actuator</uavField>
<uavField>StackRemaining-CallbackScheduler0</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve1>
<plotCurve10>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Guidance</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve10>
<plotCurve11>
<color>4294945280</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Watchdog</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve11>
<plotCurve2>
<color>4294945280</color>
<color>4278190335</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-TelemetryTx</uavField>
<uavField>StackRemaining-CallbackScheduler1</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2310,9 +2290,9 @@
<yScalePower>0</yScalePower>
</plotCurve2>
<plotCurve3>
<color>4294945280</color>
<color>4294967040</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-TelemetryTxPri</uavField>
<uavField>StackRemaining-ManualControl</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2320,9 +2300,9 @@
<yScalePower>0</yScalePower>
</plotCurve3>
<plotCurve4>
<color>4294945280</color>
<color>4278255615</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-TelemetryRx</uavField>
<uavField>StackRemaining-Stabilization</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2330,9 +2310,9 @@
<yScalePower>0</yScalePower>
</plotCurve4>
<plotCurve5>
<color>4294945280</color>
<color>4294923775</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-GPS</uavField>
<uavField>StackRemaining-Actuator</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2340,9 +2320,9 @@
<yScalePower>0</yScalePower>
</plotCurve5>
<plotCurve6>
<color>4294945280</color>
<color>4289331327</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-ManualControl</uavField>
<uavField>StackRemaining-Sensors</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2360,9 +2340,9 @@
<yScalePower>0</yScalePower>
</plotCurve7>
<plotCurve8>
<color>4294945280</color>
<color>4283760767</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-AHRSComms</uavField>
<uavField>StackRemaining-TelemetryTx</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
@ -2370,16 +2350,26 @@
<yScalePower>0</yScalePower>
</plotCurve8>
<plotCurve9>
<color>4294945280</color>
<color>4294901760</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-Stabilization</uavField>
<uavField>StackRemaining-TelemetryRx</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve9>
<plotCurveCount>12</plotCurveCount>
<plotCurve10>
<color>4283782527</color>
<mathFunction>None</mathFunction>
<uavField>StackRemaining-RadioRx</uavField>
<uavObject>TaskInfo</uavObject>
<yMaximum>0</yMaximum>
<yMeanSamples>1</yMeanSamples>
<yMinimum>0</yMinimum>
<yScalePower>0</yScalePower>
</plotCurve10>
<plotCurveCount>11</plotCurveCount>
<plotType>1</plotType>
<refreshInterval>1000</refreshInterval>
</data>

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

20
ground/openpilotgcs/share/openpilotgcs/stylesheets/default_linux.qss
View File

@ -1,19 +1,5 @@
/* Linux styles */
QGroupBox {
border: 1px solid gray;
border-radius: 5px;
margin-top: 1ex;
font-size: 11px;
font-weight: bold;
}
QGroupBox::title {
subcontrol-origin: margin;
subcontrol-position: top left;
padding: 0 3px;
}
MixerCurveWidget {
font-size: 12px;
}
QToolTip {
color: black;
}

20
ground/openpilotgcs/share/openpilotgcs/stylesheets/default_macos.qss
View File

@ -1,22 +1,2 @@
/* MacOS styles */
QGroupBox {
border: 1px solid gray;
border-radius: 5px;
margin-top: 1ex;
font-size: 11px;
font-weight: bold;
}
QGroupBox::title {
subcontrol-origin: margin;
subcontrol-position: top left;
padding: 0 3px;
}
QTabWidget::pane {
margin: 1px, 1px, 1px, 1px;
border: 2px solid rgb(196, 196, 196);
border-radius: 5px;
padding: 0px;
}

10900
ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts
View File

File diff suppressed because it is too large Load Diff

30
ground/openpilotgcs/share/openpilotgcs/translations/translations.pro
View File

@ -71,22 +71,20 @@ qmfiles.CONFIG += no_check_exist
INSTALLS += qmfiles
#========= begin block copying qt_*.qm files ==========
win32 {
defineReplace(QtQmExists) {
for(lang,$$1) {
qm_file = $$[QT_INSTALL_TRANSLATIONS]/qt_$${lang}.qm
exists($$qm_file) : result += $$qm_file
}
return($$result)
defineReplace(QtQmExists) {
for(lang,$$1) {
qm_file = $$[QT_INSTALL_TRANSLATIONS]/qt_$${lang}.qm
exists($$qm_file) : result += $$qm_file
}
QT_TRANSLATIONS = $$QtQmExists(LANGUAGES)
copyQT_QMs.input = QT_TRANSLATIONS
copyQT_QMs.output = $$GCS_DATA_PATH/translations/${QMAKE_FILE_BASE}.qm
isEmpty(vcproj):copyQT_QMs.variable_out = PRE_TARGETDEPS
copyQT_QMs.commands = $(COPY_FILE) ${QMAKE_FILE_IN} ${QMAKE_FILE_OUT}
copyQT_QMs.name = Copy ${QMAKE_FILE_IN}
copyQT_QMs.CONFIG += no_link
QMAKE_EXTRA_COMPILERS += copyQT_QMs
return($$result)
}
QT_TRANSLATIONS = $$QtQmExists(LANGUAGES)
copyQT_QMs.input = QT_TRANSLATIONS
copyQT_QMs.output = $$GCS_DATA_PATH/translations/${QMAKE_FILE_BASE}.qm
isEmpty(vcproj):copyQT_QMs.variable_out = PRE_TARGETDEPS
copyQT_QMs.commands = $(COPY_FILE) ${QMAKE_FILE_IN} ${QMAKE_FILE_OUT}
copyQT_QMs.name = Copy ${QMAKE_FILE_IN}
copyQT_QMs.CONFIG += no_link
QMAKE_EXTRA_COMPILERS += copyQT_QMs
#========= end block copying qt_*.qm files ============

169
ground/openpilotgcs/src/app/Info.plist
View File

@ -2,175 +2,6 @@
<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
<plist version="1.0">
<dict>
<key>CFBundleDocumentTypes</key>
<array>
<dict>
<key>CFBundleTypeRole</key>
<string>Editor</string>
<key>CFBundleTypeIconFile</key>
<string>profile.icns</string>
<key>CFBundleTypeExtensions</key>
<array>
<string>pro</string>
</array>
<key>CFBundleTypeName</key>
<string>Qt Project File</string>
<key>LSHandlerRank</key>
<string>Default</string>
</dict>
<dict>
<key>CFBundleTypeRole</key>
<string>Editor</string>
<key>CFBundleTypeIconFile</key>
<string>prifile.icns</string>
<key>CFBundleTypeExtensions</key>
<array>
<string>pri</string>
</array>
<key>CFBundleTypeName</key>
<string>Qt Project Include File</string>
<key>LSHandlerRank</key>
<string>Default</string>
</dict>
<dict>
<key>CFBundleTypeRole</key>
<string>Editor</string>
<key>CFBundleTypeExtensions</key>
<array>
<string>qrc</string>
</array>
<key>CFBundleTypeName</key>
<string>Qt Resource File</string>
<key>LSHandlerRank</key>
<string>Default</string>
</dict>
<dict>
<key>CFBundleTypeRole</key>
<string>Editor</string>
<key>CFBundleTypeExtensions</key>
<array>
<string>ui</string>
</array>
<key>CFBundleTypeName</key>
<string>Qt UI File</string>
</dict>
<dict>
<key>CFBundleTypeExtensions</key>
<array>
<string>h</string>
<string>hpp</string>
</array>
<key>CFBundleTypeName</key>
<string>Header File</string>
<key>CFBundleTypeOSTypes</key>
<array>
<string>TEXT</string>
<string>utxt</string>
</array>
<key>CFBundleTypeRole</key>
<string>Editor</string>
</dict>
<dict>
<key>CFBundleTypeExtensions</key>
<array>
<string>cc</string>
<string>CC</string>
<string>cp</string>
<string>CP</string>
<string>cpp</string>
<string>CPP</string>
<string>cxx</string>
<string>CXX</string>
<string>c++</string>
<string>C++</string>
</array>
<key>CFBundleTypeName</key>
<string>C++ Source File</string>
<key>CFBundleTypeOSTypes</key>
<array>
<string>TEXT</string>
<string>utxt</string>
</array>
<key>CFBundleTypeRole</key>
<string>Editor</string>
</dict>
<dict>
<key>CFBundleTypeExtensions</key>
<array>
<string>mm</string>
<string>MM</string>
</array>
<key>CFBundleTypeName</key>
<string>Objective-C++ Source File</string>
<key>CFBundleTypeOSTypes</key>
<array>
<string>TEXT</string>
<string>utxt</string>
</array>
<key>CFBundleTypeRole</key>
<string>Editor</string>
</dict>
<dict>
<key>CFBundleTypeExtensions</key>
<array>
<string>m</string>
</array>
<key>CFBundleTypeName</key>
<string>Objective-C Source File</string>
<key>CFBundleTypeOSTypes</key>
<array>
<string>TEXT</string>
<string>utxt</string>
</array>
<key>CFBundleTypeRole</key>
<string>Editor</string>
</dict>
<dict>
<key>CFBundleTypeExtensions</key>
<array>
<string>c</string>
<string>C</string>
</array>
<key>CFBundleTypeName</key>
<string>C Source File</string>
<key>CFBundleTypeOSTypes</key>
<array>
<string>TEXT</string>
<string>utxt</string>
</array>
<key>CFBundleTypeRole</key>
<string>Editor</string>
</dict>
<dict>
<key>CFBundleTypeExtensions</key>
<array>
<string>txt</string>
<string>text</string>
</array>
<key>CFBundleTypeName</key>
<string>Text File</string>
<key>CFBundleTypeOSTypes</key>
<array>
<string>TEXT</string>
</array>
<key>CFBundleTypeRole</key>
<string>Editor</string>
</dict>
<dict>
<key>CFBundleTypeExtensions</key>
<array>
<string>*</string>
</array>
<key>CFBundleTypeName</key>
<string>NSStringPboardType</string>
<key>CFBundleTypeOSTypes</key>
<array>
<string>****</string>
</array>
<key>CFBundleTypeRole</key>
<string>Editor</string>
</dict>
</array>
<key>CFBundleGetInfoString</key>
<string>OpenPilot GCS; Copyright OpenPilot</string>
<key>CFBundleIconFile</key>

1
ground/openpilotgcs/src/app/main.cpp
View File

@ -211,6 +211,7 @@ inline QString msgSendArgumentFailed()
void mainMessageOutput(QtMsgType type, const QMessageLogContext &context, const QString &msg)
{
Q_UNUSED(context);
QFile file(QDir::tempPath() + "/gcs.log");
if (file.open(QIODevice::Append | QIODevice::Text)) {

4
ground/openpilotgcs/src/libs/eigen/.hg_archival.txt Normal file
View File

@ -0,0 +1,4 @@
repo: 8a21fd850624c931e448cbcfb38168cb2717c790
node: ffa86ffb557094721ca71dcea6aed2651b9fd610
branch: 3.2
tag: 3.2.0

8
ground/openpilotgcs/src/libs/eigen/.hgeol Normal file
View File

@ -0,0 +1,8 @@
[patterns]
scripts/*.in = LF
debug/msvc/*.dat = CRLF
unsupported/test/mpreal/*.* = CRLF
** = native
[repository]
native = LF

32
ground/openpilotgcs/src/libs/eigen/.hgignore Normal file
View File

@ -0,0 +1,32 @@
syntax: glob
qrc_*cxx
*.orig
*.pyc
*.diff
diff
*.save
save
*.old
*.gmo
*.qm
core
core.*
*.bak
*~
build*
*.moc.*
*.moc
ui_*
CMakeCache.txt
tags
.*.swp
activity.png
*.out
*.php*
*.log
*.orig
*.rej
log
patch
a
a.*

25
ground/openpilotgcs/src/libs/eigen/.hgtags Normal file
View File

@ -0,0 +1,25 @@
2db9468678c6480c9633b6272ff0e3599d1e11a3 2.0-beta3
375224817dce669b6fa31d920d4c895a63fabf32 2.0-beta1
3b8120f077865e2a072e10f5be33e1d942b83a06 2.0-rc1
19dfc0e7666bcee26f7a49eb42f39a0280a3485e 2.0-beta5
7a7d8a9526f003ffa2430dfb0c2c535b5add3023 2.0-beta4
7d14ad088ac23769c349518762704f0257f6a39b 2.0.1
b9d48561579fd7d4c05b2aa42235dc9de6484bf2 2.0-beta6
e17630a40408243cb1a51ad0fe3a99beb75b7450 before-hg-migration
eda654d4cda2210ce80719addcf854773e6dec5a 2.0.0
ee9a7c468a9e73fab12f38f02bac24b07f29ed71 2.0-beta2
d49097c25d8049e730c254a2fed725a240ce4858 after-hg-migration
655348878731bcb5d9bbe0854077b052e75e5237 actual-start-from-scratch
12a658962d4e6dfdc9a1c350fe7b69e36e70675c 3.0-beta1
5c4180ad827b3f869b13b1d82f5a6ce617d6fcee 3.0-beta2
7ae24ca6f3891d5ac58ddc7db60ad413c8d6ec35 3.0-beta3
c40708b9088d622567fecc9208ad4a426621d364 3.0-beta4
b6456624eae74f49ae8683d8e7b2882a2ca0342a 3.0-rc1
a810d5dbab47acfe65b3350236efdd98f67d4d8a 3.1.0-alpha1
304c88ca3affc16dd0b008b1104873986edd77af 3.1.0-alpha2
920fc730b5930daae0a6dbe296d60ce2e3808215 3.1.0-beta1
8383e883ebcc6f14695ff0b5e20bb631abab43fb 3.1.0-rc1
bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2
da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1
4b687cad1d23066f66863f4f87298447298443df 3.2-rc1
1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2

420
ground/openpilotgcs/src/libs/eigen/CMakeLists.txt Normal file
View File

@ -0,0 +1,420 @@
project(Eigen)
cmake_minimum_required(VERSION 2.8.2)
# guard against in-source builds
if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
endif()
# guard against bad build-type strings
if (NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE "Release")
endif()
string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
if( NOT cmake_build_type_tolower STREQUAL "debug"
AND NOT cmake_build_type_tolower STREQUAL "release"
AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo")
message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are Debug, Release, RelWithDebInfo (case-insensitive).")
endif()
#############################################################################
# retrieve version infomation #
#############################################################################
# automatically parse the version number
file(READ "${PROJECT_SOURCE_DIR}/Eigen/src/Core/util/Macros.h" _eigen_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen_world_version_match "${_eigen_version_header}")
set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen_major_version_match "${_eigen_version_header}")
set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen_minor_version_match "${_eigen_version_header}")
set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN_VERSION_NUMBER ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
# if the mercurial program is absent, this will leave the EIGEN_HG_CHANGESET string empty,
# but won't stop CMake.
execute_process(COMMAND hg tip -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_HGTIP_OUTPUT)
execute_process(COMMAND hg branch -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_BRANCH_OUTPUT)
# if this is the default (aka development) branch, extract the mercurial changeset number from the hg tip output...
if(EIGEN_BRANCH_OUTPUT MATCHES "default")
string(REGEX MATCH "^changeset: *[0-9]*:([0-9;a-f]+).*" EIGEN_HG_CHANGESET_MATCH "${EIGEN_HGTIP_OUTPUT}")
set(EIGEN_HG_CHANGESET "${CMAKE_MATCH_1}")
endif(EIGEN_BRANCH_OUTPUT MATCHES "default")
#...and show it next to the version number
if(EIGEN_HG_CHANGESET)
set(EIGEN_VERSION "${EIGEN_VERSION_NUMBER} (mercurial changeset ${EIGEN_HG_CHANGESET})")
else(EIGEN_HG_CHANGESET)
set(EIGEN_VERSION "${EIGEN_VERSION_NUMBER}")
endif(EIGEN_HG_CHANGESET)
include(CheckCXXCompilerFlag)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
#############################################################################
# find how to link to the standard libraries #
#############################################################################
find_package(StandardMathLibrary)
set(EIGEN_TEST_CUSTOM_LINKER_FLAGS "" CACHE STRING "Additional linker flags when linking unit tests.")
set(EIGEN_TEST_CUSTOM_CXX_FLAGS "" CACHE STRING "Additional compiler flags when compiling unit tests.")
set(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO "")
if(NOT STANDARD_MATH_LIBRARY_FOUND)
message(FATAL_ERROR
"Can't link to the standard math library. Please report to the Eigen developers, telling them about your platform.")
else()
if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO)
set(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO "${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${STANDARD_MATH_LIBRARY}")
else()
set(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO "${STANDARD_MATH_LIBRARY}")
endif()
endif()
if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO)
message(STATUS "Standard libraries to link to explicitly: ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO}")
else()
message(STATUS "Standard libraries to link to explicitly: none")
endif()
option(EIGEN_BUILD_BTL "Build benchmark suite" OFF)
if(NOT WIN32)
option(EIGEN_BUILD_PKGCONFIG "Build pkg-config .pc file for Eigen" ON)
endif(NOT WIN32)
set(CMAKE_INCLUDE_CURRENT_DIR ON)
option(EIGEN_SPLIT_LARGE_TESTS "Split large tests into smaller executables" ON)
option(EIGEN_DEFAULT_TO_ROW_MAJOR "Use row-major as default matrix storage order" OFF)
if(EIGEN_DEFAULT_TO_ROW_MAJOR)
add_definitions("-DEIGEN_DEFAULT_TO_ROW_MAJOR")
endif()
set(EIGEN_TEST_MAX_SIZE "320" CACHE STRING "Maximal matrix/vector size, default is 320")
macro(ei_add_cxx_compiler_flag FLAG)
string(REGEX REPLACE "-" "" SFLAG ${FLAG})
check_cxx_compiler_flag(${FLAG} COMPILER_SUPPORT_${SFLAG})
if(COMPILER_SUPPORT_${SFLAG})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}")
endif()
endmacro(ei_add_cxx_compiler_flag)
if(NOT MSVC)
# We assume that other compilers are partly compatible with GNUCC
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fexceptions")
set(CMAKE_CXX_FLAGS_DEBUG "-g3")
set(CMAKE_CXX_FLAGS_RELEASE "-g0 -O2")
# clang outputs some warnings for unknwon flags that are not caught by check_cxx_compiler_flag
# adding -Werror turns such warnings into errors
check_cxx_compiler_flag("-Werror" COMPILER_SUPPORT_WERROR)
if(COMPILER_SUPPORT_WERROR)
set(CMAKE_REQUIRED_FLAGS "-Werror")
endif()
ei_add_cxx_compiler_flag("-pedantic")
ei_add_cxx_compiler_flag("-Wall")
ei_add_cxx_compiler_flag("-Wextra")
#ei_add_cxx_compiler_flag("-Weverything") # clang
ei_add_cxx_compiler_flag("-Wundef")
ei_add_cxx_compiler_flag("-Wcast-align")
ei_add_cxx_compiler_flag("-Wchar-subscripts")
ei_add_cxx_compiler_flag("-Wnon-virtual-dtor")
ei_add_cxx_compiler_flag("-Wunused-local-typedefs")
ei_add_cxx_compiler_flag("-Wpointer-arith")
ei_add_cxx_compiler_flag("-Wwrite-strings")
ei_add_cxx_compiler_flag("-Wformat-security")
ei_add_cxx_compiler_flag("-Wno-psabi")
ei_add_cxx_compiler_flag("-Wno-variadic-macros")
ei_add_cxx_compiler_flag("-Wno-long-long")
ei_add_cxx_compiler_flag("-fno-check-new")
ei_add_cxx_compiler_flag("-fno-common")
ei_add_cxx_compiler_flag("-fstrict-aliasing")
ei_add_cxx_compiler_flag("-wd981") # disable ICC's "operands are evaluated in unspecified order" remark
ei_add_cxx_compiler_flag("-wd2304") # disbale ICC's "warning #2304: non-explicit constructor with single argument may cause implicit type conversion" produced by -Wnon-virtual-dtor
# The -ansi flag must be added last, otherwise it is also used as a linker flag by check_cxx_compiler_flag making it fails
# Moreover we should not set both -strict-ansi and -ansi
check_cxx_compiler_flag("-strict-ansi" COMPILER_SUPPORT_STRICTANSI)
ei_add_cxx_compiler_flag("-Qunused-arguments") # disable clang warning: argument unused during compilation: '-ansi'
if(COMPILER_SUPPORT_STRICTANSI)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -strict-ansi")
else()
ei_add_cxx_compiler_flag("-ansi")
endif()
set(CMAKE_REQUIRED_FLAGS "")
option(EIGEN_TEST_SSE2 "Enable/Disable SSE2 in tests/examples" OFF)
if(EIGEN_TEST_SSE2)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse2")
message(STATUS "Enabling SSE2 in tests/examples")
endif()
option(EIGEN_TEST_SSE3 "Enable/Disable SSE3 in tests/examples" OFF)
if(EIGEN_TEST_SSE3)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse3")
message(STATUS "Enabling SSE3 in tests/examples")
endif()
option(EIGEN_TEST_SSSE3 "Enable/Disable SSSE3 in tests/examples" OFF)
if(EIGEN_TEST_SSSE3)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mssse3")
message(STATUS "Enabling SSSE3 in tests/examples")
endif()
option(EIGEN_TEST_SSE4_1 "Enable/Disable SSE4.1 in tests/examples" OFF)
if(EIGEN_TEST_SSE4_1)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse4.1")
message(STATUS "Enabling SSE4.1 in tests/examples")
endif()
option(EIGEN_TEST_SSE4_2 "Enable/Disable SSE4.2 in tests/examples" OFF)
if(EIGEN_TEST_SSE4_2)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse4.2")
message(STATUS "Enabling SSE4.2 in tests/examples")
endif()
option(EIGEN_TEST_ALTIVEC "Enable/Disable AltiVec in tests/examples" OFF)
if(EIGEN_TEST_ALTIVEC)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -maltivec -mabi=altivec")
message(STATUS "Enabling AltiVec in tests/examples")
endif()
option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF)
if(EIGEN_TEST_NEON)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a"8)
message(STATUS "Enabling NEON in tests/examples")
endif()
check_cxx_compiler_flag("-fopenmp" COMPILER_SUPPORT_OPENMP)
if(COMPILER_SUPPORT_OPENMP)
option(EIGEN_TEST_OPENMP "Enable/Disable OpenMP in tests/examples" OFF)
if(EIGEN_TEST_OPENMP)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
message(STATUS "Enabling OpenMP in tests/examples")
endif()
endif()
else(NOT MSVC)
# C4127 - conditional expression is constant
# C4714 - marked as __forceinline not inlined (I failed to deactivate it selectively)
# We can disable this warning in the unit tests since it is clear that it occurs
# because we are oftentimes returning objects that have a destructor or may
# throw exceptions - in particular in the unit tests we are throwing extra many
# exceptions to cover indexing errors.
# C4505 - unreferenced local function has been removed (impossible to deactive selectively)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /wd4127 /wd4505 /wd4714")
# replace all /Wx by /W4
string(REGEX REPLACE "/W[0-9]" "/W4" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
check_cxx_compiler_flag("/openmp" COMPILER_SUPPORT_OPENMP)
if(COMPILER_SUPPORT_OPENMP)
option(EIGEN_TEST_OPENMP "Enable/Disable OpenMP in tests/examples" OFF)
if(EIGEN_TEST_OPENMP)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /openmp")
message(STATUS "Enabling OpenMP in tests/examples")
endif()
endif()
option(EIGEN_TEST_SSE2 "Enable/Disable SSE2 in tests/examples" OFF)
if(EIGEN_TEST_SSE2)
if(NOT CMAKE_CL_64)
# arch is not supported on 64 bit systems, SSE is enabled automatically.
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /arch:SSE2")
endif(NOT CMAKE_CL_64)
message(STATUS "Enabling SSE2 in tests/examples")
endif(EIGEN_TEST_SSE2)
endif(NOT MSVC)
option(EIGEN_TEST_NO_EXPLICIT_VECTORIZATION "Disable explicit vectorization in tests/examples" OFF)
option(EIGEN_TEST_X87 "Force using X87 instructions. Implies no vectorization." OFF)
option(EIGEN_TEST_32BIT "Force generating 32bit code." OFF)
if(EIGEN_TEST_X87)
set(EIGEN_TEST_NO_EXPLICIT_VECTORIZATION ON)
if(CMAKE_COMPILER_IS_GNUCXX)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpmath=387")
message(STATUS "Forcing use of x87 instructions in tests/examples")
else()
message(STATUS "EIGEN_TEST_X87 ignored on your compiler")
endif()
endif()
if(EIGEN_TEST_32BIT)
if(CMAKE_COMPILER_IS_GNUCXX)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -m32")
message(STATUS "Forcing generation of 32-bit code in tests/examples")
else()
message(STATUS "EIGEN_TEST_32BIT ignored on your compiler")
endif()
endif()
if(EIGEN_TEST_NO_EXPLICIT_VECTORIZATION)
add_definitions(-DEIGEN_DONT_VECTORIZE=1)
message(STATUS "Disabling vectorization in tests/examples")
endif()
option(EIGEN_TEST_NO_EXPLICIT_ALIGNMENT "Disable explicit alignment (hence vectorization) in tests/examples" OFF)
if(EIGEN_TEST_NO_EXPLICIT_ALIGNMENT)
add_definitions(-DEIGEN_DONT_ALIGN=1)
message(STATUS "Disabling alignment in tests/examples")
endif()
option(EIGEN_TEST_C++0x "Enables all C++0x features." OFF)
include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
# the user modifiable install path for header files
set(EIGEN_INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} CACHE PATH "The directory where we install the header files (optional)")
# set the internal install path for header files which depends on wether the user modifiable
# EIGEN_INCLUDE_INSTALL_DIR has been set by the user or not.
if(EIGEN_INCLUDE_INSTALL_DIR)
set(INCLUDE_INSTALL_DIR
${EIGEN_INCLUDE_INSTALL_DIR}
CACHE INTERNAL
"The directory where we install the header files (internal)"
)
else()
set(INCLUDE_INSTALL_DIR
"${CMAKE_INSTALL_PREFIX}/include/eigen3"
CACHE INTERNAL
"The directory where we install the header files (internal)"
)
endif()
# similar to set_target_properties but append the property instead of overwriting it
macro(ei_add_target_property target prop value)
get_target_property(previous ${target} ${prop})
# if the property wasn't previously set, ${previous} is now "previous-NOTFOUND" which cmake allows catching with plain if()
if(NOT previous)
set(previous "")
endif(NOT previous)
set_target_properties(${target} PROPERTIES ${prop} "${previous} ${value}")
endmacro(ei_add_target_property)
install(FILES
signature_of_eigen3_matrix_library
DESTINATION ${INCLUDE_INSTALL_DIR} COMPONENT Devel
)
if(EIGEN_BUILD_PKGCONFIG)
SET(path_separator ":")
STRING(REPLACE ${path_separator} ";" pkg_config_libdir_search "$ENV{PKG_CONFIG_LIBDIR}")
message(STATUS "searching for 'pkgconfig' directory in PKG_CONFIG_LIBDIR ( $ENV{PKG_CONFIG_LIBDIR} ), ${CMAKE_INSTALL_PREFIX}/share, and ${CMAKE_INSTALL_PREFIX}/lib")
FIND_PATH(pkg_config_libdir pkgconfig ${pkg_config_libdir_search} ${CMAKE_INSTALL_PREFIX}/share ${CMAKE_INSTALL_PREFIX}/lib ${pkg_config_libdir_search})
if(pkg_config_libdir)
SET(pkg_config_install_dir ${pkg_config_libdir})
message(STATUS "found ${pkg_config_libdir}/pkgconfig" )
else(pkg_config_libdir)
SET(pkg_config_install_dir ${CMAKE_INSTALL_PREFIX}/share)
message(STATUS "pkgconfig not found; installing in ${pkg_config_install_dir}" )
endif(pkg_config_libdir)
configure_file(eigen3.pc.in eigen3.pc)
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc
DESTINATION ${pkg_config_install_dir}/pkgconfig
)
endif(EIGEN_BUILD_PKGCONFIG)
add_subdirectory(Eigen)
add_subdirectory(doc EXCLUDE_FROM_ALL)
include(EigenConfigureTesting)
# fixme, not sure this line is still needed:
enable_testing() # must be called from the root CMakeLists, see man page
if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
else()
add_subdirectory(test EXCLUDE_FROM_ALL)
endif()
if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
add_subdirectory(blas)
add_subdirectory(lapack)
else()
add_subdirectory(blas EXCLUDE_FROM_ALL)
add_subdirectory(lapack EXCLUDE_FROM_ALL)
endif()
add_subdirectory(unsupported)
add_subdirectory(demos EXCLUDE_FROM_ALL)
# must be after test and unsupported, for configuring buildtests.in
add_subdirectory(scripts EXCLUDE_FROM_ALL)
# TODO: consider also replacing EIGEN_BUILD_BTL by a custom target "make btl"?
if(EIGEN_BUILD_BTL)
add_subdirectory(bench/btl EXCLUDE_FROM_ALL)
endif(EIGEN_BUILD_BTL)
if(NOT WIN32)
add_subdirectory(bench/spbench EXCLUDE_FROM_ALL)
endif(NOT WIN32)
configure_file(scripts/cdashtesting.cmake.in cdashtesting.cmake @ONLY)
ei_testing_print_summary()
message(STATUS "")
message(STATUS "Configured Eigen ${EIGEN_VERSION_NUMBER}")
message(STATUS "")
option(EIGEN_FAILTEST "Enable failtests." OFF)
if(EIGEN_FAILTEST)
add_subdirectory(failtest)
endif()
string(TOLOWER "${CMAKE_GENERATOR}" cmake_generator_tolower)
if(cmake_generator_tolower MATCHES "makefile")
message(STATUS "Some things you can do now:")
message(STATUS "--------------+--------------------------------------------------------------")
message(STATUS "Command | Description")
message(STATUS "--------------+--------------------------------------------------------------")
message(STATUS "make install | Install to ${CMAKE_INSTALL_PREFIX}. To change that:")
message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourpath")
message(STATUS " | Eigen headers will then be installed to:")
message(STATUS " | ${INCLUDE_INSTALL_DIR}")
message(STATUS " | To install Eigen headers to a separate location, do:")
message(STATUS " | cmake . -DEIGEN_INCLUDE_INSTALL_DIR=yourpath")
message(STATUS "make doc | Generate the API documentation, requires Doxygen & LaTeX")
message(STATUS "make check | Build and run the unit-tests. Read this page:")
message(STATUS " | http://eigen.tuxfamily.org/index.php?title=Tests")
message(STATUS "make blas | Build BLAS library (not the same thing as Eigen)")
message(STATUS "--------------+--------------------------------------------------------------")
else()
message(STATUS "To build/run the unit tests, read this page:")
message(STATUS " http://eigen.tuxfamily.org/index.php?title=Tests")
endif()
message(STATUS "")

Some files were not shown because too many files have changed in this diff Show More