mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-18 08:54:15 +01:00
Merge remote-tracking branch 'origin/next' into filnet/OP-1194_persist_scope_gadget_plot_and_legend_visibility_state
This commit is contained in:
commit
03bdd5e03e
118
WHATSNEW.txt
118
WHATSNEW.txt
@ -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.
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
|
@ -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));
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
|
@ -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)
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
@ -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]);
|
||||
}
|
||||
}
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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
133
flight/modules/ManualControl/altitudehandler.c
Normal 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
326
flight/modules/ManualControl/armhandler.c
Normal 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);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -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
|
||||
|
@ -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
71
flight/modules/ManualControl/manualhandler.c
Normal 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
131
flight/modules/ManualControl/pathfollowerhandler.c
Normal 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
64
flight/modules/ManualControl/pathplannerhandler.c
Normal 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
143
flight/modules/ManualControl/stabilizedhandler.c
Normal 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);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -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,
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
@ -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
673
flight/modules/Receiver/receiver.c
Normal 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
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -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;
|
||||
|
@ -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;
|
||||
|
@ -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];
|
||||
|
@ -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;
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
@ -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) {
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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;
|
||||
|
@ -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
|
@ -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
64
flight/pios/common/pios_deltatime.c
Normal 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;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -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;
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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
53
flight/pios/inc/pios_deltatime.h
Normal 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 */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -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
|
||||
|
@ -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>
|
||||
|
@ -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
|
||||
|
@ -36,7 +36,6 @@
|
||||
#include <utlist.h>
|
||||
#include <uavobjectmanager.h>
|
||||
#include <eventdispatcher.h>
|
||||
#include <callbackscheduler.h>
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
|
@ -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
|
||||
|
||||
|
@ -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
|
||||
|
@ -198,7 +198,7 @@ void PIOS_Board_Init(void)
|
||||
}
|
||||
|
||||
/* Initialize the delayed callback library */
|
||||
CallbackSchedulerInitialize();
|
||||
PIOS_CALLBACKSCHEDULER_Initialize();
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
|
@ -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();
|
||||
|
@ -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
|
||||
|
@ -37,7 +37,6 @@
|
||||
#include <utlist.h>
|
||||
#include <uavobjectmanager.h>
|
||||
#include <eventdispatcher.h>
|
||||
#include <callbackscheduler.h>
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
|
@ -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 */
|
||||
|
@ -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
|
||||
|
@ -106,7 +106,7 @@ void PIOS_Board_Init(void)
|
||||
}
|
||||
|
||||
/* Initialize the delayed callback library */
|
||||
CallbackSchedulerInitialize();
|
||||
PIOS_CALLBACKSCHEDULER_Initialize();
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
|
@ -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();
|
||||
|
@ -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
|
||||
|
@ -36,7 +36,6 @@
|
||||
#include <utlist.h>
|
||||
#include <uavobjectmanager.h>
|
||||
#include <eventdispatcher.h>
|
||||
#include <callbackscheduler.h>
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
|
@ -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 */
|
||||
|
@ -178,7 +178,7 @@ void PIOS_Board_Init(void)
|
||||
}
|
||||
|
||||
/* Initialize the delayed callback library */
|
||||
CallbackSchedulerInitialize();
|
||||
PIOS_CALLBACKSCHEDULER_Initialize();
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -37,7 +37,6 @@
|
||||
#include <utlist.h>
|
||||
#include <uavobjectmanager.h>
|
||||
#include <eventdispatcher.h>
|
||||
#include <callbackscheduler.h>
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
|
@ -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 */
|
||||
|
@ -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
|
||||
|
@ -404,7 +404,7 @@ void PIOS_Board_Init(void)
|
||||
}
|
||||
|
||||
/* Initialize the delayed callback library */
|
||||
CallbackSchedulerInitialize();
|
||||
PIOS_CALLBACKSCHEDULER_Initialize();
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
|
@ -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();
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
||||
|
||||
|
@ -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
|
||||
|
@ -37,7 +37,6 @@
|
||||
#include <utlist.h>
|
||||
#include <uavobjectmanager.h>
|
||||
#include <eventdispatcher.h>
|
||||
#include <callbackscheduler.h>
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
|
@ -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 */
|
||||
|
@ -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
|
||||
|
@ -444,7 +444,7 @@ void PIOS_Board_Init(void)
|
||||
}
|
||||
|
||||
/* Initialize the delayed callback library */
|
||||
CallbackSchedulerInitialize();
|
||||
PIOS_CALLBACKSCHEDULER_Initialize();
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
|
@ -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();
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -37,7 +37,6 @@
|
||||
#include <utlist.h>
|
||||
#include <uavobjectmanager.h>
|
||||
#include <eventdispatcher.h>
|
||||
#include <callbackscheduler.h>
|
||||
#include <uavtalk.h>
|
||||
|
||||
#include "alarms.h"
|
||||
|
@ -36,6 +36,7 @@
|
||||
|
||||
/* Major features */
|
||||
#define PIOS_INCLUDE_FREERTOS
|
||||
#define PIOS_INCLUDE_CALLBACKSCHEDULER
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
|
||||
/* Enable/Disable PiOS Modules */
|
||||
|
@ -132,7 +132,7 @@ void PIOS_Board_Init(void)
|
||||
}
|
||||
|
||||
/* Initialize the delayed callback library */
|
||||
CallbackSchedulerInitialize();
|
||||
PIOS_CALLBACKSCHEDULER_Initialize();
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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 */
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
|
@ -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)
|
||||
|
@ -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>
|
||||
|
@ -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>
|
||||
|
@ -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>
|
||||
|
@ -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"
|
||||
|
Before Width: | Height: | Size: 235 KiB After Width: | Height: | Size: 235 KiB |
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
}
|
||||
|
File diff suppressed because it is too large
Load Diff
@ -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 ============
|
||||
|
@ -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>
|
||||
|
@ -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
4
ground/openpilotgcs/src/libs/eigen/.hg_archival.txt
Normal 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
8
ground/openpilotgcs/src/libs/eigen/.hgeol
Normal 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
32
ground/openpilotgcs/src/libs/eigen/.hgignore
Normal 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
25
ground/openpilotgcs/src/libs/eigen/.hgtags
Normal 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
420
ground/openpilotgcs/src/libs/eigen/CMakeLists.txt
Normal 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
Loading…
x
Reference in New Issue
Block a user