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

Merge branch 'next' into corvuscorax/OP-1161_magnetometer-alarm

Browse Source 
Conflicts:
	shared/uavobjectdefinition/revosettings.xml
	shared/uavobjectdefinition/systemalarms.xml
This commit is contained in:
Corvus Corax 2014-05-01 10:26:53 +02:00
commit b6b03b767a
1885 changed files with 252162 additions and 66562 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
.commit-template Normal file
View File

@ -0,0 +1,23 @@
# *************** OpenPilot commits guidelines ***************
# Each commit needs to have a message like the following sample:
# OP-1150 UI for thermal calibration: Connect State machine to UI
#
# It needs to begin with a reference to one or more Jira tickets followed by a short description.
# If needed add a longer description in the following lines, after an empty line.
#
# Before committing, ensure your code is properly formatted using:
# make uncrustify_all
# You can format flight or ground code only using respectively
# uncrustify_flight or uncrustify_ground
#
# To automatically create a review, append the following smart commit messages:
# +review OPReview
#
# To append the commit to an existing review, use the following smart commit message:
# +review OPReview-NNN
# For example "+review OPReview-609"
#
# *NOTE* leave an empty line between the commit message and "smart commit command"
# Smart commits commands need to starts immediately at first column

7
CREDITS.txt
View File

@ -5,7 +5,9 @@ Pedro Assuncao
Fredrik Arvidsson
Werner Backes
Jose Barros
Mikael Blomqvist
Pete Boehl
Glenn Campigli
David Carlson
James Cotton
Steve Doll
@ -13,7 +15,9 @@ Piotr Esden-Tempski
Richard Flay
Peter Farnworth
Ed Faulkner
Andrew Finegan
Darren Furniss
Cliff Geerdes
Frederic Goddeeris
Daniel Godin
Anthony Gomez
@ -24,6 +28,7 @@ Peter Gunnarsson
Dean Hall
Joe Hlebasko
Andy Honecker
Patrick Huebner
Ryan Hunt
Mark James
Ricky King
@ -34,6 +39,7 @@ Alan Krum
Edouard Lafargue
Mike Labranche
Fredrik Larsson
Richard von Lehe
Pablo Lema
David Llama
Matt Lipski
@ -65,6 +71,7 @@ Troy Schultz
Dr. Erhard Siegl
Dusty Anne Smith
Mike Smith
Bertrand Songis
Alex Sowa
Pete Stapley
Vova Starikh

32
Makefile
View File

@ -450,11 +450,25 @@ else
GCS_SILENT := silent
endif
.NOTPARALLEL:
.PHONY: openpilotgcs
openpilotgcs: uavobjects_gcs
$(V1) $(MKDIR) -p $(BUILD_DIR)/$@_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/$@_$(GCS_BUILD_CONF) && \
$(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/openpilotgcs.pro -spec $(QT_SPEC) -r CONFIG+="$(GCS_BUILD_CONF) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) && \
openpilotgcs: uavobjects_gcs openpilotgcs_qmake openpilotgcs_make
.PHONY: openpilotgcs_qmake
openpilotgcs_qmake:
ifeq ($(QMAKE_SKIP),)
$(V1) $(MKDIR) -p $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF) && \
$(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/openpilotgcs.pro -spec $(QT_SPEC) -r CONFIG+="$(GCS_BUILD_CONF) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) \
)
else
@$(ECHO) "skipping qmake"
endif
.PHONY: openpilotgcs_make
openpilotgcs_make:
$(V1) $(MKDIR) -p $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/$(MAKE_DIR) && \
$(MAKE) -w ; \
)
@ -709,7 +723,7 @@ $(OPFW_RESOURCE): $(FW_TARGETS)
# If opfw_resource or all firmware are requested, GCS should depend on the resource
ifneq ($(strip $(filter opfw_resource all all_fw all_flight,$(MAKECMDGOALS))),)
$(eval openpilotgcs: $(OPFW_RESOURCE))
$(eval openpilotgcs_qmake: $(OPFW_RESOURCE))
endif
# Packaging targets: package, clean_package
@ -859,12 +873,13 @@ help:
@$(ECHO)
@$(ECHO) " Here is a summary of the available targets:"
@$(ECHO)
@$(ECHO) " [Source tree preparation]"
@$(ECHO) " prepare - Install GIT commit message template"
@$(ECHO) " [Tool Installers]"
@$(ECHO) " arm_sdk_install - Install the GNU ARM gcc toolchain"
@$(ECHO) " qt_sdk_install - Install the QT development tools"
@$(ECHO) " mingw_install - Install the MinGW toolchain (Windows only)"
@$(ECHO) " python_install - Install the Python interpreter (Windows only)"
@$(ECHO) " nsis_install - Install the NSIS Unicode (Windows only)"
@$(ECHO) " sdl_install - Install the SDL library (Windows only)"
@$(ECHO) " openssl_install - Install the OpenSSL libraries (Windows only)"
@$(ECHO) " uncrustify_install - Install the Uncrustify source code beautifier"
@$(ECHO) " doxygen_install - Install the Doxygen documentation generator"
@ -945,6 +960,9 @@ help:
@$(ECHO)
@$(ECHO) " [GCS]"
@$(ECHO) " gcs - Build the Ground Control System (GCS) application (debug|release)"
@$(ECHO) " Skip qmake: QMAKE_SKIP=1"
@$(ECHO) " Compile specific directory: MAKE_DIR=<dir>"
@$(ECHO) " Example: make gcs QMAKE_SKIP=1 MAKE_DIR=src/plugins/coreplugin"
@$(ECHO) " gcs_clean - Remove the Ground Control System (GCS) application (debug|release)"
@$(ECHO) " Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
@$(ECHO)

133
WHATSNEW.txt
View File

@ -1,3 +1,132 @@
--- 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
Telemetry and OPLink reliability.
Some additions in this release:
- "Rattitude" flight mode;
- Altitude Hold Reimplementation;
- Multiple PID banks;
- "Cruise Control"
the full list of features, improvements and bufixes shipping
in this release is accessible here:
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.
- Fixed issues with Google Maps;
@ -6,7 +135,8 @@ This maintenance release includes the following fixes missing in (previously not
JIRA issues addressed in this maintenance release:
OP-1044, OP-1070, OP-1072
Use the following link for a comprehensive list of issues addressed by this release
http://progress.openpilot.org/browse/OP-1070
http://progress.openpilot.org/issues/?filter=11060
--- RELEASE-13.06.03 ---
@ -296,6 +426,7 @@ GCS code changes:
- added OPLinkMini configuration page;
- hardware options are now dynamically enabled/disabled to allow supported configurations only;
- new artwork for all boards everywhere;
- optimised some 3D models;
- new About dialog showing version info and contributors list;
- fixed badly broken HiTL options dialog;

0
artwork/3D Model/boards/Revolution/Revolution.3DS → artwork/3D Model/boards/Revolution/revolution.3ds
View File

38
flight/libraries/alarms.c
View File

@ -32,11 +32,17 @@
#include "inc/alarms.h"
// Private constants
#ifndef PIOS_ALARM_GRACETIME
// alarm cannot be turned off for at least 1000 milliseconds
// to prevent event system overload through flapping alarms
#define PIOS_ALARM_GRACETIME 1000
#endif // PIOS_ALARM_GRACETIME
// Private types
// Private variables
static xSemaphoreHandle lock;
static volatile uint16_t lastAlarmChange[SYSTEMALARMS_ALARM_NUMELEM] = { 0 }; // this deliberately overflows every 2^16 milliseconds to save memory
// Private functions
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
@ -63,7 +69,7 @@ int32_t AlarmsInitialize(void)
*/
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
SystemAlarmsAlarmData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) {
@ -74,10 +80,14 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) {
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
SystemAlarmsSet(&alarms);
SystemAlarmsAlarmGet(&alarms);
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if ((flightTime - lastAlarmChange[alarm] > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms, alarms.Actuator)[alarm] != severity)
|| cast_struct_to_array(alarms, alarms.Actuator)[alarm] < severity) {
cast_struct_to_array(alarms, alarms.Actuator)[alarm] = severity;
lastAlarmChange[alarm] = flightTime;
SystemAlarmsAlarmSet(&alarms);
}
// Release lock
@ -110,10 +120,14 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) {
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if ((flightTime - lastAlarmChange[alarm] > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity)
|| cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] < severity) {
cast_struct_to_array(alarms.ExtendedAlarmStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = status;
cast_struct_to_array(alarms.ExtendedAlarmSubStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = subStatus;
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
lastAlarmChange[alarm] = flightTime;
SystemAlarmsSet(&alarms);
}
@ -129,7 +143,7 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
*/
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
{
SystemAlarmsData alarms;
SystemAlarmsAlarmData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) {
@ -137,8 +151,8 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
}
// Read alarm
SystemAlarmsGet(&alarms);
return cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm];
SystemAlarmsAlarmGet(&alarms);
return cast_struct_to_array(alarms, alarms.Actuator)[alarm];
}
/**
@ -220,17 +234,17 @@ int32_t AlarmsHasCritical()
*/
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
SystemAlarmsAlarmData alarms;
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarms
SystemAlarmsGet(&alarms);
SystemAlarmsAlarmGet(&alarms);
// Go through alarms and check if any are of the given severity or higher
for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[n] >= severity) {
if (cast_struct_to_array(alarms, alarms.Actuator)[n] >= severity) {
xSemaphoreGiveRecursive(lock);
return 1;
}

9
flight/libraries/math/sin_lookup.c
View File

@ -105,7 +105,14 @@ float sin_lookup_deg(float angle)
return 0;
}
int i_ang = ((int32_t)angle) % 360;
// <bug, was> int i_ang = ((int32_t)angle) % 360;
// 1073741760 is a multiple of 360 that is close to 0x3fffffff
// so angle can be a very large number of positive or negative rotations
// this is the fastest fix (no tests, one integer addition)
// and has the largest range since float mantissas are 23-4 bit
// we could halve the lookup table size
// we could interpolate for greater accuracy
int i_ang = ((int32_t)(angle + 0.5f) + (int32_t)1073741760) % 360;
if (i_ang >= 180) { // for 180 to 360 deg
return -sin_table[i_ang - 180];
} else { // for 0 to 179 deg

64
flight/libraries/sanitycheck.c
View File

@ -34,7 +34,9 @@
// UAVOs
#include <manualcontrolsettings.h>
#include <flightmodesettings.h>
#include <systemsettings.h>
#include <systemalarms.h>
#include <taskinfo.h>
/****************************
@ -84,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;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold
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)) {
@ -132,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)) {
@ -140,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)) {
@ -148,15 +150,21 @@ int32_t configuration_check()
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports PathPlanner
severity = SYSTEMALARMS_ALARM_ERROR;
} else {
// Revo supports PathPlanner and that must be OK or we are not sane
// PathPlan alarm is uninitialized if not running
// PathPlan alarm is warning or error if the flightplan is invalid
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
@ -194,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;
@ -219,14 +227,14 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
// For multirotors verify that nothing is set to "none"
if (multirotor) {
for (uint32_t i = 0; i < NELEMENTS(modes); i++) {
if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) {
return SYSTEMALARMS_ALARM_ERROR;
}
}
}
// Warning: This assumes that certain conditions in the XML file are met. That
// MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE
return SYSTEMALARMS_ALARM_OK;

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

@ -55,7 +55,7 @@
#define STACK_SIZE_BYTES 1312
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // device driver
#define FAILSAFE_TIMEOUT_MS 100
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
@ -129,6 +129,9 @@ int32_t ActuatorInitialize()
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
ActuatorDesiredConnectQueue(queue);
// Register AccessoryDesired (Secondary input to this module)
AccessoryDesiredInitialize();
// Primary output of this module
ActuatorCommandInitialize();
@ -166,6 +169,9 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
ActuatorDesiredData desired;
MixerStatusData mixerStatus;
FlightStatusData flightStatus;
SystemSettingsThrustControlOptions thrustType;
float throttleDesired;
float collectiveDesired;
/* Read initial values of ActuatorSettings */
ActuatorSettingsData actuatorSettings;
@ -220,6 +226,41 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
FlightStatusGet(&flightStatus);
ActuatorDesiredGet(&desired);
ActuatorCommandGet(&command);
SystemSettingsThrustControlGet(&thrustType);
// read in throttle and collective -demultiplex thrust
switch (thrustType) {
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
throttleDesired = desired.Thrust;
ManualControlCommandCollectiveGet(&collectiveDesired);
break;
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
ManualControlCommandThrottleGet(&throttleDesired);
collectiveDesired = desired.Thrust;
break;
default:
ManualControlCommandThrottleGet(&throttleDesired);
ManualControlCommandCollectiveGet(&collectiveDesired);
}
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
// safety settings
if (!armed) {
throttleDesired = 0;
}
if (throttleDesired <= 0.00f || !armed) {
// force set all other controls to zero if throttle is cut (previously set in Stabilization)
if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Roll = 0;
}
if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Pitch = 0;
}
if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Yaw = 0;
}
}
#ifdef DIAG_MIXERSTATUS
MixerStatusGet(&mixerStatus);
@ -238,18 +279,18 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool positiveThrottle = desired.Throttle >= 0.00f;
bool activeThrottle = (throttleDesired < 0.00f || throttleDesired > 0.00f);
bool positiveThrottle = (throttleDesired > 0.00f);
bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
float curve1 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
// The source for the secondary curve is selectable
float curve2 = 0;
AccessoryDesiredData accessory;
switch (mixerSettings.Curve2Source) {
case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
curve2 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
curve2 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_ROLL:
curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
@ -262,8 +303,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
ManualControlCommandCollectiveGet(&curve2);
curve2 = MixerCurve(curve2, mixerSettings.ThrottleCurve2,
curve2 = MixerCurve(collectiveDesired, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
@ -295,7 +335,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
continue;
}
if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds);
} else {
status[ct] = -1;
@ -317,6 +357,16 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
}
}
// Reversable Motors are like Motors but go to neutral instead of minimum
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
// If not armed or motor is inactive - no "spinwhilearmed" for this engine type
if (!armed || !activeThrottle) {
filterAccumulator[ct] = 0;
lastResult[ct] = 0;
status[ct] = 0; // force neutral throttle
}
}
// If an accessory channel is selected for direct bypass mode
// In this configuration the accessory channel is scaled and mapped
// directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
@ -419,6 +469,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
// note: no feedforward for reversable motors yet for safety reasons
if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
if (result < 0.0f) { // idle throttle
result = 0.0f;
@ -537,7 +588,7 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
Channel[n] = actuatorSettings->ChannelMin[n];
} else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
} else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
Channel[n] = actuatorSettings->ChannelNeutral[n];
} else {
Channel[n] = 0;

21
flight/modules/Airspeed/airspeed.c
View File

@ -41,9 +41,11 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "baro_airspeed_ms4525do.h"
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_mpxv.h"
#include "gps_airspeed.h"
#include "airspeedalarm.h"
#include "taskinfo.h"
// Private constants
@ -59,7 +61,7 @@
static xTaskHandle taskHandle;
static bool airspeedEnabled = false;
static AirspeedSettingsData airspeedSettings;
static AirspeedSettingsAirspeedSensorTypeOptions lastAirspeedSensorType = 0;
static int8_t airspeedADCPin = -1;
@ -118,6 +120,8 @@ int32_t AirspeedInitialize()
}
}
lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
AirspeedSensorInitialize();
AirspeedSettingsInitialize();
@ -144,6 +148,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
// Main task loop
portTickType lastSysTime = xTaskGetTickCount();
while (1) {
@ -152,6 +157,12 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
// Update the airspeed object
AirspeedSensorGet(&airspeedData);
// if sensor type changed reset Airspeed alarm
if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) {
AirspeedAlarm(SYSTEMALARMS_ALARM_DEFAULT);
lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
}
switch (airspeedSettings.AirspeedSensorType) {
#if defined(PIOS_INCLUDE_MPXV)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
@ -165,12 +176,20 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
// Eagletree Airspeed v3
baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
break;
#endif
#if defined(PIOS_INCLUDE_MS4525DO)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO:
// PixHawk Airpeed based on MS4525DO
baro_airspeedGetMS4525DO(&airspeedData, &airspeedSettings);
break;
#endif
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
gps_airspeedGet(&airspeedData, &airspeedSettings);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
default:
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
break;
}
// Set the UAVO

65
flight/modules/Airspeed/airspeedalarm.c Normal file
View File

@ -0,0 +1,65 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Handle locally airspeed alarms issue changes to PIOS only when necessary
* @{
*
* @file airspeedalarm.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module
*
* @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
*/
/**
* Output object: none
*
* Handle locally airspeed alarms issue changes to PIOS only when necessary
*
*/
#include "airspeedalarm.h"
// local variable
static SystemAlarmsAlarmOptions severitySet = SYSTEMALARMS_ALARM_UNINITIALISED;
// functions
/**
* Handle airspeed alarms and isuue an Alarm to PIOS only if necessary
*/
bool AirspeedAlarm(SystemAlarmsAlarmOptions severity)
{
if (severity == severitySet) {
return false;
}
severitySet = severity;
return AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, severity) == 0;
}
/**
* @}
* @}
*/

4
flight/modules/Airspeed/baro_airspeed_etasv3.c
View File

@ -40,6 +40,7 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#if defined(PIOS_INCLUDE_ETASV3)
@ -64,11 +65,13 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
if (airspeedSensor->SensorValue == (uint16_t)-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
// only calibrate if no stored calibration is available
if (!airspeedSettings->ZeroPoint) {
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
// Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
@ -93,6 +96,7 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
// Compute airspeed
airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint));
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}

6
flight/modules/Airspeed/baro_airspeed_mpxv.c
View File

@ -40,6 +40,7 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#if defined(PIOS_INCLUDE_MPXV)
@ -63,7 +64,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
// Ensure that the ADC pin is properly configured
if (airspeedADCPin < 0) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
if (sensor.type == PIOS_MPXV_UNKNOWN) {
@ -76,6 +77,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
break;
default:
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
}
@ -83,6 +85,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor);
if (!airspeedSettings->ZeroPoint) {
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
// Calibrate sensor by averaging zero point value
if (calibrationCount < CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { // First let sensor warm up and stabilize.
calibrationCount++;
@ -107,6 +110,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
#endif /* if defined(PIOS_INCLUDE_MPXV) */

140
flight/modules/Airspeed/baro_airspeed_ms4525do.c Normal file
View File

@ -0,0 +1,140 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Communicate with airspeed sensors and return values
* @{
*
* @file baro_airspeed_ms4525do.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module, handles temperature and pressure readings from MS4525DO
*
* @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
*/
/**
* Output object: BaroAirspeed
*
* This module will periodically update the value of the BaroAirspeed object.
*
*/
#include "openpilot.h"
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#include "taskinfo.h"
#if defined(PIOS_INCLUDE_MS4525DO)
#define CALIBRATION_IDLE_MS 0 // Time to wait before calibrating, in [ms]
#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms]
#define FILTER_SHIFT 5 // Barry Dorr filter parameter k
#define P0 101325.0f // standard pressure
#define CCEXPONENT 0.2857142857f // exponent of compressibility correction 2/7
#define CASFACTOR 760.8802669f // sqrt(5) * speed of sound at standard
#define TASFACTOR 0.05891022589f // 1/sqrt(T0)
// Private types
// Private functions definitions
// Private variables
static uint16_t calibrationCount = 0;
static uint32_t filter_reg = 0; // Barry Dorr filter register
void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings)
{
uint16_t values[2];
// Read sensor
int8_t retVal = PIOS_MS4525DO_Read(values);
// check for errors
if (retVal != 0) {
airspeedSensor->SensorValue = -1;
airspeedSensor->SensorValueTemperature = -1;
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
airspeedSensor->SensorValue = values[0];
airspeedSensor->SensorValueTemperature = values[1];
// only calibrate if no stored calibration is available
if (!airspeedSettings->ZeroPoint) {
// Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT);
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
return;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
calibrationCount++;
// update filter register
filter_reg = filter_reg - (filter_reg >> FILTER_SHIFT) + airspeedSensor->SensorValue;
if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
// Scale output for unity gain.
airspeedSettings->ZeroPoint = (uint16_t)(filter_reg >> FILTER_SHIFT);
AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint);
calibrationCount = 0;
}
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
return;
}
}
/* Compute airspeed
assume sensor is A Type and has a range of 1 psi, i.e. Pmin=-1.0 psi and Pmax=1.0 psi
Datasheet pressure: output = 0.8 * 16383 / (Pmax-Pmin) * (P - Pmin) + 0.1 * 16383
Inversion: P = (10*output - 81915)/65532 in psi
1 psi = 6894,757293168 Pa
P = (10*output - 81915)*0.1052120688 in Pa
Datasheet temperature: output = (T+50)*2047 / 200
Inversion: T = (200*out - 102350)/2047 in C
T = (200*out - 102350)/2047 + 273.15 in K
*/
const float dP = (10 * (int32_t)(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint)) * 0.1052120688f;
const float T = (float)(200 * (int32_t)airspeedSensor->SensorValueTemperature - 102350) / 2047 + 273.15f;
airspeedSensor->DifferentialPressure = dP;
airspeedSensor->Temperature = T;
// CAS = Csound * sqrt( 5 *( (dP/P0 +1)^(2/7) - 1) )
// TAS = Csound * sqrt( 5 T/T0 *( (dP/P0 +1)^(2/7) - 1) )
// where Csound = 340.276 m/s at standard condition T0=288.15 K and P0 = 101315 Pa
airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * CASFACTOR * sqrtf(powf(fabsf(dP) / P0 + 1.0f, CCEXPONENT) - 1.0f);
airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(T);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
// everything is fine so set ALARM_OK
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
#endif /* if defined(PIOS_INCLUDE_MS4525DO) */
/**
* @}
* @}
*/

4
flight/modules/Airspeed/gps_airspeed.c
View File

@ -37,6 +37,7 @@
#include "airspeedsettings.h"
#include "gps_airspeed.h"
#include "CoordinateConversions.h"
#include "airspeedalarm.h"
#include <pios_math.h>
@ -127,6 +128,7 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air
if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return; // do not calculate if gps velocity is insufficient...
}
@ -141,11 +143,13 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
} else {
// need a low pass filter to filter out spikes in non coordinated maneuvers
airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed;
gps->oldAirspeed = airspeedData->CalibratedAirspeed;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
// Save old variables for next pass

27
flight/modules/FlightPlan/inc/flightplan.h → flight/modules/Airspeed/inc/airspeedalarm.h
View File

@ -2,13 +2,13 @@
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup FlightPlan Flight Plan Module
* @brief Executes flight plan scripts in Python
* @addtogroup AirspeedModule Airspeed Module
* @brief Handle locally airspeed alarms issue changes to PIOS only when necessary
* @{
*
* @file flightplan.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Executes flight plan scripts in Python
* @file airspeedalarm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module, reads temperature and pressure from MS4525DO
*
* @see The GNU Public License (GPL) Version 3
*
@ -28,9 +28,18 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef FLIGHTPLAN_H
#define FLIGHTPLAN_H
#ifndef AIRSPEEDALARM_H
#define AIRSPEEDALARM_H
int32_t FlightPlanInitialize();
#include <openpilot.h>
#include "alarms.h"
#endif // FLIGHTPLAN_H
bool AirspeedAlarm(SystemAlarmsAlarmOptions severity);
#endif // AIRSPEEDALARM_H
/**
* @}
* @}
*/

43
flight/modules/Airspeed/inc/baro_airspeed_ms4525do.h Normal file
View File

@ -0,0 +1,43 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Calculate airspeed from measurements of differential pressure from a MS4525DO (PixHawk airspeed sensor)
* @{
*
* @file baro_airspeed_mas4525do.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module, reads temperature and pressure from MS4525DO
*
* @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 BARO_AIRSPEED_MS4525DO_H
#define BARO_AIRSPEED_MS4525DO_H
#if defined(PIOS_INCLUDE_MS4525DO)
void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings);
#endif /* PIOS_INCLUDE_MS4525DO */
#endif // BARO_AIRSPEED_MS4525DO_H
/**
* @}
* @}
*/

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

@ -40,22 +40,26 @@
#include "altitude.h"
#include "barosensor.h" // object that will be updated by the module
#include "revosettings.h"
#if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module
#endif
#include "taskinfo.h"
// Private constants
#define STACK_SIZE_BYTES 500
#define STACK_SIZE_BYTES 550
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
// Private types
// 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);
/**
* Initialise the module, called on startup
@ -77,6 +81,9 @@ int32_t AltitudeStart()
int32_t AltitudeInitialize()
{
BaroSensorInitialize();
RevoSettingsInitialize();
RevoSettingsConnectCallback(&SettingsUpdatedCb);
SettingsUpdatedCb(NULL);
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
#endif
@ -103,6 +110,8 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
// Undef for normal operation
// #define PIOS_MS5611_SLOW_TEMP_RATE 20
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
uint8_t temp_press_interleave_count = 1;
#endif
@ -154,12 +163,17 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
vTaskDelay(PIOS_MS5611_GetDelay());
PIOS_MS5611_ReadADC();
temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure();
float altitude = 44330.0f * (1.0f - powf(press / MS5611_P0, (1.0f / 5.255f)));
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)) {
data.Altitude = altitude;
@ -171,6 +185,13 @@ 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));
}
/**
* @}
* @}

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

@ -45,34 +45,37 @@
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <altholdsmoothed.h>
#include <attitudestate.h>
#include <altitudeholdsettings.h>
#include <altitudeholddesired.h> // object that will be updated by the module
#include <barosensor.h>
#include <positionstate.h>
#include <altitudeholdstatus.h>
#include <flightstatus.h>
#include <stabilizationdesired.h>
#include <accelstate.h>
#include <taskinfo.h>
#include <pios_constants.h>
#include <velocitystate.h>
#include <positionstate.h>
// Private constants
#define MAX_QUEUE_SIZE 2
#define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define ACCEL_DOWNSAMPLE 4
#define TIMEOUT_TRESHOLD 200000
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define STACK_SIZE_BYTES 1024
#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
// Private types
// Private variables
static xTaskHandle altitudeHoldTaskHandle;
static xQueueHandle queue;
static DelayedCallbackInfo *altitudeHoldCBInfo;
static AltitudeHoldSettingsData altitudeHoldSettings;
static struct pid pid0, pid1;
// Private functions
static void altitudeHoldTask(void *parameters);
static void altitudeHoldTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
/**
@ -82,8 +85,8 @@ static void SettingsUpdatedCb(UAVObjEvent *ev);
int32_t AltitudeHoldStart()
{
// Start main task
xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle);
SettingsUpdatedCb(NULL);
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
return 0;
}
@ -96,319 +99,107 @@ int32_t AltitudeHoldInitialize()
{
AltitudeHoldSettingsInitialize();
AltitudeHoldDesiredInitialize();
AltHoldSmoothedInitialize();
AltitudeHoldStatusInitialize();
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
return 0;
}
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
float tau;
float throttleIntegral;
float velocity;
float decay;
float velocity_decay;
bool running = false;
float error;
float switchThrottle;
float smoothed_altitude;
float starting_altitude;
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(__attribute__((unused)) void *parameters)
static void altitudeHoldTask(void)
{
AltitudeHoldDesiredData altitudeHoldDesired;
StabilizationDesiredData stabilizationDesired;
static float startThrust = 0.5f;
portTickType thisTime, lastUpdateTime;
UAVObjEvent ev;
// make sure we run only when we are supposed to run
FlightStatusData flightStatus;
// Force update of the settings
SettingsUpdatedCb(&ev);
// Failsafe handling
uint32_t lastAltitudeHoldDesiredUpdate = 0;
bool enterFailSafe = false;
// Listen for updates.
AltitudeHoldDesiredConnectQueue(queue);
BaroSensorConnectQueue(queue);
FlightStatusConnectQueue(queue);
AccelStateConnectQueue(queue);
bool altitudeHoldFlightMode = false;
BaroSensorAltitudeGet(&smoothed_altitude);
running = false;
enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
break;
default:
pid_zero(&pid0);
pid_zero(&pid1);
StabilizationDesiredThrustGet(&startThrust);
PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
uint8_t flightMode;
FlightStatusFlightModeGet(&flightMode);
// initialize enable flag
altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
// Main task loop
bool baro_updated = false;
while (1) {
enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) {
if (!running) {
throttleIntegral = 0;
}
// Todo: Add alarm if it should be running
continue;
} else if (ev.obj == BaroSensorHandle()) {
baro_updated = true;
init = (init == WAITING_BARO) ? WAITIING_INIT : init;
} else if (ev.obj == FlightStatusHandle()) {
FlightStatusFlightModeGet(&flightMode);
altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
if (altitudeHoldFlightMode && !running) {
// Copy the current throttle as a starting point for integral
StabilizationDesiredThrottleGet(&throttleIntegral);
switchThrottle = throttleIntegral;
error = 0;
velocity = 0;
running = true;
AltHoldSmoothedData altHold;
AltHoldSmoothedGet(&altHold);
starting_altitude = altHold.Altitude;
} else if (!altitudeHoldFlightMode) {
running = false;
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
}
} else if (ev.obj == AccelStateHandle()) {
static uint32_t timeval;
static float z[4] = { 0, 0, 0, 0 };
float z_new[4];
float P[4][4], K[4][2], x[2];
float G[4] = { 1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f };
static float V[4][4] = {
{ 10.0f, 0.0f, 0.0f, 0.0f },
{ 0.0f, 100.0f, 0.0f, 0.0f },
{ 0.0f, 0.0f, 100.0f, 0.0f },
{ 0.0f, 0.0f, 0.0f, 1000.0f }
};
static uint32_t accel_downsample_count = 0;
static float accels_accum[3] = { 0.0f, 0.0f, 0.0f };
float dT;
static float S[2] = { 1.0f, 10.0f };
/* Somehow this always assigns to zero. Compiler bug? Race condition? */
S[0] = altitudeHoldSettings.PressureNoise;
S[1] = altitudeHoldSettings.AccelNoise;
G[2] = altitudeHoldSettings.AccelDrift;
AccelStateData accelState;
AccelStateGet(&accelState);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
BaroSensorData baro;
BaroSensorGet(&baro);
/* Downsample accels to stop this calculation consuming too much CPU */
accels_accum[0] += accelState.x;
accels_accum[1] += accelState.y;
accels_accum[2] += accelState.z;
accel_downsample_count++;
if (accel_downsample_count < ACCEL_DOWNSAMPLE) {
continue;
}
accel_downsample_count = 0;
accelState.x = accels_accum[0] / ACCEL_DOWNSAMPLE;
accelState.y = accels_accum[1] / ACCEL_DOWNSAMPLE;
accelState.z = accels_accum[2] / ACCEL_DOWNSAMPLE;
accels_accum[0] = accels_accum[1] = accels_accum[2] = 0;
thisTime = xTaskGetTickCount();
if (init == WAITIING_INIT) {
z[0] = baro.Altitude;
z[1] = 0;
z[2] = accelState.z;
z[3] = 0;
init = INITED;
} else if (init == WAITING_BARO) {
continue;
}
x[0] = baro.Altitude;
// rotate avg accels into earth frame and store it
if (1) {
float q[4], Rbe[3][3];
q[0] = attitudeState.q1;
q[1] = attitudeState.q2;
q[2] = attitudeState.q3;
q[3] = attitudeState.q4;
Quaternion2R(q, Rbe);
x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f);
} else {
x[1] = -accelState.z + 9.81f;
}
dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;
timeval = PIOS_DELAY_GetRaw();
P[0][0] = dT * (V[0][1] + dT * V[1][1]) + V[0][0] + G[0] + dT * V[1][0];
P[0][1] = dT * (V[0][2] + dT * V[1][2]) + V[0][1] + dT * V[1][1];
P[0][2] = V[0][2] + dT * V[1][2];
P[0][3] = V[0][3] + dT * V[1][3];
P[1][0] = dT * (V[1][1] + dT * V[2][1]) + V[1][0] + dT * V[2][0];
P[1][1] = dT * (V[1][2] + dT * V[2][2]) + V[1][1] + G[1] + dT * V[2][1];
P[1][2] = V[1][2] + dT * V[2][2];
P[1][3] = V[1][3] + dT * V[2][3];
P[2][0] = V[2][0] + dT * V[2][1];
P[2][1] = V[2][1] + dT * V[2][2];
P[2][2] = V[2][2] + G[2];
P[2][3] = V[2][3];
P[3][0] = V[3][0] + dT * V[3][1];
P[3][1] = V[3][1] + dT * V[3][2];
P[3][2] = V[3][2];
P[3][3] = V[3][3] + G[3];
if (baro_updated) {
K[0][0] = -(V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) + 1.0f;
K[0][1] = ((V[0][2] + V[0][3]) * S[0] + dT * (V[1][2] + V[1][3]) * S[0]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[1][0] = (V[1][0] * G[2] + V[1][0] * G[3] + V[1][0] * S[1] + V[1][0] * V[2][2] - V[2][0] * V[1][2] + V[1][0] * V[2][3] + V[1][0] * V[3][2] - V[2][0] * V[1][3] - V[1][2] * V[3][0] + V[1][0] * V[3][3] - V[3][0] * V[1][3] + (dT * dT) * V[2][1] * V[3][2] - (dT * dT) * V[2][2] * V[3][1] + (dT * dT) * V[2][1] * V[3][3] - (dT * dT) * V[3][1] * V[2][3] + dT * V[1][1] * G[2] + dT * V[2][0] * G[2] + dT * V[1][1] * G[3] + dT * V[2][0] * G[3] + dT * V[1][1] * S[1] + dT * V[2][0] * S[1] + (dT * dT) * V[2][1] * G[2] + (dT * dT) * V[2][1] * G[3] + (dT * dT) * V[2][1] * S[1] + dT * V[1][1] * V[2][2] - dT * V[1][2] * V[2][1] + dT * V[1][1] * V[2][3] + dT * V[1][1] * V[3][2] + dT * V[2][0] * V[3][2] - dT * V[1][2] * V[3][1] - dT * V[2][1] * V[1][3] - dT * V[3][0] * V[2][2] + dT * V[1][1] * V[3][3] + dT * V[2][0] * V[3][3] - dT * V[3][0] * V[2][3] - dT * V[1][3] * V[3][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[1][1] = (V[1][2] * G[0] + V[1][3] * G[0] + V[1][2] * S[0] + V[1][3] * S[0] + V[0][0] * V[1][2] - V[1][0] * V[0][2] + V[0][0] * V[1][3] - V[1][0] * V[0][3] + (dT * dT) * V[0][1] * V[2][2] + (dT * dT) * V[1][0] * V[2][2] - (dT * dT) * V[0][2] * V[2][1] - (dT * dT) * V[2][0] * V[1][2] + (dT * dT) * V[0][1] * V[2][3] + (dT * dT) * V[1][0] * V[2][3] - (dT * dT) * V[2][0] * V[1][3] - (dT * dT) * V[0][3] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][2] - (dT * dT * dT) * V[1][2] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][3] - (dT * dT * dT) * V[2][1] * V[1][3] + dT * V[2][2] * G[0] + dT * V[2][3] * G[0] + dT * V[2][2] * S[0] + dT * V[2][3] * S[0] + dT * V[0][0] * V[2][2] + dT * V[0][1] * V[1][2] - dT * V[0][2] * V[1][1] - dT * V[0][2] * V[2][0] + dT * V[0][0] * V[2][3] + dT * V[0][1] * V[1][3] - dT * V[1][1] * V[0][3] - dT * V[2][0] * V[0][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[2][0] = (V[2][0] * G[3] - V[3][0] * G[2] + V[2][0] * S[1] + V[2][0] * V[3][2] - V[3][0] * V[2][2] + V[2][0] * V[3][3] - V[3][0] * V[2][3] + dT * V[2][1] * G[3] - dT * V[3][1] * G[2] + dT * V[2][1] * S[1] + dT * V[2][1] * V[3][2] - dT * V[2][2] * V[3][1] + dT * V[2][1] * V[3][3] - dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[2][1] = (V[0][0] * G[2] + V[2][2] * G[0] + V[2][3] * G[0] + V[2][2] * S[0] + V[2][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] - V[2][0] * V[0][3] + G[0] * G[2] + G[2] * S[0] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] - (dT * dT) * V[2][1] * V[1][3] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + (dT * dT) * V[1][1] * G[2] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[1][0] * V[2][3] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[3][0] = (-V[2][0] * G[3] + V[3][0] * G[2] + V[3][0] * S[1] - V[2][0] * V[3][2] + V[3][0] * V[2][2] - V[2][0] * V[3][3] + V[3][0] * V[2][3] - dT * V[2][1] * G[3] + dT * V[3][1] * G[2] + dT * V[3][1] * S[1] - dT * V[2][1] * V[3][2] + dT * V[2][2] * V[3][1] - dT * V[2][1] * V[3][3] + dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
K[3][1] = (V[0][0] * G[3] + V[3][2] * G[0] + V[3][3] * G[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[3][2] - V[0][2] * V[3][0] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[3] + G[3] * S[0] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + (dT * dT) * V[1][1] * G[3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
z_new[0] = -K[0][0] * (dT * z[1] - x[0] + z[0]) + dT * z[1] - K[0][1] * (-x[1] + z[2] + z[3]) + z[0];
z_new[1] = -K[1][0] * (dT * z[1] - x[0] + z[0]) + dT * z[2] - K[1][1] * (-x[1] + z[2] + z[3]) + z[1];
z_new[2] = -K[2][0] * (dT * z[1] - x[0] + z[0]) - K[2][1] * (-x[1] + z[2] + z[3]) + z[2];
z_new[3] = -K[3][0] * (dT * z[1] - x[0] + z[0]) - K[3][1] * (-x[1] + z[2] + z[3]) + z[3];
memcpy(z, z_new, sizeof(z_new));
V[0][0] = -K[0][1] * P[2][0] - K[0][1] * P[3][0] - P[0][0] * (K[0][0] - 1.0f);
V[0][1] = -K[0][1] * P[2][1] - K[0][1] * P[3][2] - P[0][1] * (K[0][0] - 1.0f);
V[0][2] = -K[0][1] * P[2][2] - K[0][1] * P[3][2] - P[0][2] * (K[0][0] - 1.0f);
V[0][3] = -K[0][1] * P[2][3] - K[0][1] * P[3][3] - P[0][3] * (K[0][0] - 1.0f);
V[1][0] = P[1][0] - K[1][0] * P[0][0] - K[1][1] * P[2][0] - K[1][1] * P[3][0];
V[1][1] = P[1][1] - K[1][0] * P[0][1] - K[1][1] * P[2][1] - K[1][1] * P[3][2];
V[1][2] = P[1][2] - K[1][0] * P[0][2] - K[1][1] * P[2][2] - K[1][1] * P[3][2];
V[1][3] = P[1][3] - K[1][0] * P[0][3] - K[1][1] * P[2][3] - K[1][1] * P[3][3];
V[2][0] = -K[2][0] * P[0][0] - K[2][1] * P[3][0] - P[2][0] * (K[2][1] - 1.0f);
V[2][1] = -K[2][0] * P[0][1] - K[2][1] * P[3][2] - P[2][1] * (K[2][1] - 1.0f);
V[2][2] = -K[2][0] * P[0][2] - K[2][1] * P[3][2] - P[2][2] * (K[2][1] - 1.0f);
V[2][3] = -K[2][0] * P[0][3] - K[2][1] * P[3][3] - P[2][3] * (K[2][1] - 1.0f);
V[3][0] = -K[3][0] * P[0][0] - K[3][1] * P[2][0] - P[3][0] * (K[3][1] - 1.0f);
V[3][1] = -K[3][0] * P[0][1] - K[3][1] * P[2][1] - P[3][2] * (K[3][1] - 1.0f);
V[3][2] = -K[3][0] * P[0][2] - K[3][1] * P[2][2] - P[3][2] * (K[3][1] - 1.0f);
V[3][3] = -K[3][0] * P[0][3] - K[3][1] * P[2][3] - P[3][3] * (K[3][1] - 1.0f);
baro_updated = false;
} else {
K[0][0] = (V[0][2] + V[0][3] + dT * V[1][2] + dT * V[1][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
K[1][0] = (V[1][2] + V[1][3] + dT * V[2][2] + dT * V[2][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
K[2][0] = (V[2][2] + V[2][3] + G[2]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
K[3][0] = (V[3][2] + V[3][3] + G[3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
z_new[0] = dT * z[1] - K[0][0] * (-x[1] + z[2] + z[3]) + z[0];
z_new[1] = dT * z[2] - K[1][0] * (-x[1] + z[2] + z[3]) + z[1];
z_new[2] = -K[2][0] * (-x[1] + z[2] + z[3]) + z[2];
z_new[3] = -K[3][0] * (-x[1] + z[2] + z[3]) + z[3];
memcpy(z, z_new, sizeof(z_new));
V[0][0] = P[0][0] - K[0][0] * P[2][0] - K[0][0] * P[3][0];
V[0][1] = P[0][1] - K[0][0] * P[2][1] - K[0][0] * P[3][2];
V[0][2] = P[0][2] - K[0][0] * P[2][2] - K[0][0] * P[3][2];
V[0][3] = P[0][3] - K[0][0] * P[2][3] - K[0][0] * P[3][3];
V[1][0] = P[1][0] - K[1][0] * P[2][0] - K[1][0] * P[3][0];
V[1][1] = P[1][1] - K[1][0] * P[2][1] - K[1][0] * P[3][2];
V[1][2] = P[1][2] - K[1][0] * P[2][2] - K[1][0] * P[3][2];
V[1][3] = P[1][3] - K[1][0] * P[2][3] - K[1][0] * P[3][3];
V[2][0] = -K[2][0] * P[3][0] - P[2][0] * (K[2][0] - 1.0f);
V[2][1] = -K[2][0] * P[3][2] - P[2][1] * (K[2][0] - 1.0f);
V[2][2] = -K[2][0] * P[3][2] - P[2][2] * (K[2][0] - 1.0f);
V[2][3] = -K[2][0] * P[3][3] - P[2][3] * (K[2][0] - 1.0f);
V[3][0] = -K[3][0] * P[2][0] - P[3][0] * (K[3][0] - 1.0f);
V[3][1] = -K[3][0] * P[2][1] - P[3][2] * (K[3][0] - 1.0f);
V[3][2] = -K[3][0] * P[2][2] - P[3][2] * (K[3][0] - 1.0f);
V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f);
}
AltHoldSmoothedData altHold;
AltHoldSmoothedGet(&altHold);
altHold.Altitude = z[0];
altHold.Velocity = z[1];
altHold.Accel = z[2];
AltHoldSmoothedSet(&altHold);
AltHoldSmoothedGet(&altHold);
// Verify that we are in altitude hold mode
uint8_t armed;
FlightStatusArmedGet(&armed);
if (!altitudeHoldFlightMode || armed != FLIGHTSTATUS_ARMED_ARMED) {
running = false;
}
if (!running) {
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
continue;
}
// Compute the altitude error
error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude;
// Compute integral off altitude error
throttleIntegral += error * altitudeHoldSettings.Ki * dT;
// Only update stabilizationDesired less frequently
if ((thisTime - lastUpdateTime) < 20) {
continue;
}
lastUpdateTime = thisTime;
// Instead of explicit limit on integral you output limit feedback
StabilizationDesiredGet(&stabilizationDesired);
if (!enterFailSafe) {
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
if (stabilizationDesired.Throttle > 1) {
throttleIntegral -= (stabilizationDesired.Throttle - 1);
stabilizationDesired.Throttle = 1;
} else if (stabilizationDesired.Throttle < 0) {
throttleIntegral -= stabilizationDesired.Throttle;
stabilizationDesired.Throttle = 0;
}
} else {
// shutdown motors
stabilizationDesired.Throttle = -1;
}
stabilizationDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
StabilizationDesiredSet(&stabilizationDesired);
} else if (ev.obj == AltitudeHoldDesiredHandle()) {
// reset the failsafe timer
lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
AltitudeHoldDesiredGet(&altitudeHoldDesired);
}
break;
}
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
// do the actual control loop(s)
AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired);
float positionStateDown;
PositionStateDownGet(&positionStateDown);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
// altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
break;
case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
break;
}
AltitudeHoldStatusSet(&altitudeHoldStatus);
float thrust;
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST:
thrust = altitudeHoldDesired.SetPoint;
break;
default:
// velocity control loop
thrust = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
if (thrust >= 1.0f) {
thrust = 1.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.Thrust = thrust;
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stab);
PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
pid_zero(&pid0);
pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
pid_zero(&pid1);
}

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

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

25
flight/modules/Attitude/revolution/attitude.c
View File

@ -278,7 +278,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run);
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR:
case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
ret_val = updateAttitudeINSGPS(first_run, true);
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
@ -650,7 +650,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
// we have airspeed available
airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed;
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down);
airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed;
AirspeedStateSet(&airspeed);
}
}
@ -1072,13 +1072,23 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
vel[2] = gpsVelData.Down;
}
// Copy the position into the UAVO
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = Nav.Pos[0];
positionState.East = Nav.Pos[1];
positionState.Down = Nav.Pos[2];
PositionStateSet(&positionState);
// airspeed correction needs current positionState
if (airspeed_updated) {
// we have airspeed available
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - Nav.Pos[2]);
airspeed.TrueAirspeed = (airspeedData.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedData.TrueAirspeed;
AirspeedStateSet(&airspeed);
if (!gps_vel_updated && !gps_updated) {
@ -1107,14 +1117,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors);
}
// Copy the position and velocity into the UAVO
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = Nav.Pos[0];
positionState.East = Nav.Pos[1];
positionState.Down = Nav.Pos[2];
PositionStateSet(&positionState);
// Copy the velocity into the UAVO
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
velocityState.North = Nav.Vel[0];

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

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

88
flight/modules/Battery/battery.c
View File

@ -47,6 +47,7 @@
#include "openpilot.h"
#include "flightstatus.h"
#include "flightbatterystate.h"
#include "flightbatterysettings.h"
#include "hwsettings.h"
@ -69,6 +70,7 @@ static int8_t currentADCPin = -1; // ADC pin for current
// Private functions
static void onTimer(UAVObjEvent *ev);
static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData);
/**
* Initialise the module, called on startup
@ -113,6 +115,8 @@ int32_t BatteryInitialize(void)
FlightBatteryStateInitialize();
FlightBatterySettingsInitialize();
// FlightBatterySettingsConnectCallback(FlightBatterySettingsUpdatedCb);
static UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent));
@ -125,10 +129,11 @@ int32_t BatteryInitialize(void)
MODULE_INITCALL(BatteryInitialize, 0);
static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
{
static FlightBatteryStateData flightBatteryData;
static FlightBatterySettingsData batterySettings;
static FlightBatteryStateData flightBatteryData;
FlightBatterySettingsGet(&batterySettings);
FlightBatteryStateGet(&flightBatteryData);
const float dT = SAMPLE_PERIOD_MS / 1000.0f;
float energyRemaining;
@ -140,7 +145,11 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
}
if (currentADCPin >= 0) {
// voltage available: get the number of cells if possible, desired and not armed
GetNbCells(&batterySettings, &flightBatteryData);
// ad a plausibility check: zero voltage => zero current
if (currentADCPin >= 0 && flightBatteryData.Voltage > 0.f) {
flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.CurrentZero) * batterySettings.SensorCalibrations.CurrentFactor; // in Amps
if (flightBatteryData.Current > flightBatteryData.PeakCurrent) {
flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps
@ -149,7 +158,11 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
flightBatteryData.Current = -0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
}
flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); // in mAh
// For safety reasons consider only positive currents in energy comsumption, i.e. no charging up.
// necesary when sensor are not perfectly calibrated
if (flightBatteryData.Current > 0) {
flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); // in mAh
}
// Apply a 2 second rise time low-pass filter to average the current
float alpha = 1.0f - dT / (dT + 2.0f);
@ -184,9 +197,9 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
// FIXME: should make the battery voltage detection dependent on battery type.
/*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/
if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Alarm * batterySettings.NbCells) {
if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Alarm * flightBatteryData.NbCells) {
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
} else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * batterySettings.NbCells) {
} else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * flightBatteryData.NbCells) {
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
@ -196,6 +209,71 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
FlightBatteryStateSet(&flightBatteryData);
}
static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, FlightBatteryStateData *flightBatteryData)
{
// get flight status to check for armed
uint8_t armed = 0;
FlightStatusArmedGet(&armed);
// check only if not armed
if (armed == FLIGHTSTATUS_ARMED_ARMED) {
return -2;
}
// prescribed number of cells?
if (batterySettings->NbCells != 0) {
flightBatteryData->NbCells = batterySettings->NbCells;
flightBatteryData->NbCellsAutodetected = 0;
return -3;
}
// plausibility check
if (flightBatteryData->Voltage <= 0.5f) {
// cannot detect number of cells
flightBatteryData->NbCellsAutodetected = 0;
return -1;
}
float voltageMin = 0.f, voltageMax = 0.f;
// Cell type specific values
// TODO: could be implemented as constant arrays indexed by cellType
// or could be part of the UAVObject definition
switch (batterySettings->Type) {
case FLIGHTBATTERYSETTINGS_TYPE_LIPO:
case FLIGHTBATTERYSETTINGS_TYPE_LICO:
voltageMin = 3.6f;
voltageMax = 4.2f;
break;
case FLIGHTBATTERYSETTINGS_TYPE_A123:
voltageMin = 2.01f;
voltageMax = 3.59f;
break;
case FLIGHTBATTERYSETTINGS_TYPE_LIFESO4:
default:
flightBatteryData->NbCellsAutodetected = 0;
return -1;
}
// uniquely measurable under any condition iff n * voltageMax < (n+1) * voltageMin
// or n < voltageMin / (voltageMax-voltageMin)
// weaken condition by setting n <= voltageMin / (voltageMax-voltageMin) and
// checking for v <= voltageMin * voltageMax / (voltageMax-voltageMin)
if (flightBatteryData->Voltage > voltageMin * voltageMax / (voltageMax - voltageMin)) {
flightBatteryData->NbCellsAutodetected = 0;
return -1;
}
flightBatteryData->NbCells = (int8_t)(flightBatteryData->Voltage / voltageMin);
flightBatteryData->NbCellsAutodetected = 1;
return flightBatteryData->NbCells;
}
/**
* @}
*/

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

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

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

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

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

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

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

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

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

@ -49,7 +49,6 @@
#include "attitudestate.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionstate.h"
#include "manualcontrol.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "airspeedstate.h"
@ -182,52 +181,50 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
uint8_t result;
// Check the combinations of flightmode and pathdesired mode
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
break;
default:
} else {
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
@ -235,8 +232,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
courseIntegral = 0;
speedIntegral = 0;
powerIntegral = 0;
break;
}
PathStatusSet(&pathStatus);
}
@ -357,10 +352,10 @@ static void updateFixedAttitude(float *attitude)
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
@ -420,7 +415,7 @@ static uint8_t updateFixedDesiredAttitude()
/**
* Compute speed error (required for throttle and pitch)
* Compute speed error (required for thrust and pitch)
*/
// Current ground speed
@ -474,9 +469,9 @@ static uint8_t updateFixedDesiredAttitude()
}
/**
* Compute desired throttle command
* Compute desired thrust command
*/
// compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant.
// compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
powerIntegral = bound(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
@ -491,7 +486,7 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
);
// Compute final throttle response
// Compute final thrust response
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp +
powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki +
speedErrorToPowerCommandComponent;
@ -501,14 +496,14 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral;
fixedwingpathfollowerStatus.Command.Power = powerCommand;
// set throttle
stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrottleLimit.Min,
fixedwingpathfollowerSettings.ThrottleLimit.Max);
// set thrust
stabDesired.Thrust = bound(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrustLimit.Min,
fixedwingpathfollowerSettings.ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors.Lowpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Max && // throttle at maximum
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + 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 (fixedwingpathfollowerSettings.ThrustLimit.Neutral + 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
@ -560,7 +555,7 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError < 0 && // we are too fast already

1
flight/modules/FlightPlan/flightplan.c
View File

@ -31,7 +31,6 @@
#include <openpilot.h>
#include "flightplan.h"
#include "flightplanstatus.h"
#include "flightplancontrol.h"
#include "flightplansettings.h"

29
flight/modules/GPS/UBX.c
View File

@ -254,22 +254,25 @@ void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
{
if (!(timeutc->valid & TIMEUTC_VALIDUTC)) {
// Test if time is valid
if ((timeutc->valid & TIMEUTC_VALIDTOW) && (timeutc->valid & TIMEUTC_VALIDWKN)) {
// Time is valid, set GpsTime
GPSTimeData GpsTime;
GpsTime.Year = timeutc->year;
GpsTime.Month = timeutc->month;
GpsTime.Day = timeutc->day;
GpsTime.Hour = timeutc->hour;
GpsTime.Minute = timeutc->min;
GpsTime.Second = timeutc->sec;
GPSTimeSet(&GpsTime);
} else {
// Time is not valid, nothing to do
return;
}
GPSTimeData GpsTime;
GpsTime.Year = timeutc->year;
GpsTime.Month = timeutc->month;
GpsTime.Day = timeutc->day;
GpsTime.Hour = timeutc->hour;
GpsTime.Minute = timeutc->min;
GpsTime.Second = timeutc->sec;
GPSTimeSet(&GpsTime);
}
#endif
#endif /* if !defined(PIOS_GPS_MINIMAL) */
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)

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

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

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

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

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

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

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

@ -6,7 +6,7 @@
* @{
*
* @file manualcontrol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief ManualControl module. Handles safety R/C link and flight mode.
*
* @see The GNU Public License (GPL) Version 3
@ -30,30 +30,57 @@
#ifndef MANUALCONTROL_H
#define MANUALCONTROL_H
#include "manualcontrolcommand.h"
#include <openpilot.h>
#include <flightstatus.h>
typedef enum { FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4 } flightmode_path;
typedef void (*handlerFunc)(bool newinit);
#define PARSE_FLIGHT_MODE(x) \
( \
(x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \
(x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \
FLIGHTMODE_UNDEFINED \
)
typedef struct controlHandlerStruct {
FlightStatusControlChainData controlChain;
handlerFunc handler;
} controlHandler;
int32_t ManualControlInitialize();
/**
* @brief Handler to interprete Command inputs in regards to arming/disarming (called in all flight modes)
* @input: ManualControlCommand, AccessoryDesired
* @output: FlightStatus.Arming
*/
void armHandler(bool newinit);
/**
* @brief Handler to control Manual flightmode - input directly steers actuators
* @input: ManualControlCommand
* @output: ActuatorDesired
*/
void manualHandler(bool newinit);
/**
* @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
* @input: ManualControlCommand
* @output: StabilizationDesired
*/
void stabilizedHandler(bool newinit);
/**
* @brief Handler to control deprecated flight modes controlled by AltitudeHold module
* @input: ManualControlCommand
* @output: AltitudeHoldDesired
*/
void altitudeHandler(bool newinit);
/**
* @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
* @output: PathDesired
*/
void pathFollowerHandler(bool newinit);
/**
* @brief Handler to control Navigated flightmodes. FlightControl is governed by PathFollower, controlled indirectly via PathPlanner
* @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands to affect navigation
* @output: NONE
*/
void pathPlannerHandler(bool newinit);
/*
* These are assumptions we make in the flight code about the order of settings and their consistency between
@ -61,54 +88,46 @@ int32_t ManualControlInitialize();
*/
#define assumptions1 \
( \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions3 \
( \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions5 \
( \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions_flightmode \
( \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \
)
#define assumptions_channelcount \
( \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \
((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM))
#endif // MANUALCONTROL_H

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

@ -11,7 +11,7 @@
* AttitudeDesired object (stabilized mode)
*
* @file manualcontrol.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief ManualControl module. Handles safety R/C link and flight mode.
*
* @see The GNU Public License (GPL) Version 3
@ -33,29 +33,16 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "accessorydesired.h"
#include "actuatordesired.h"
#include "altitudeholddesired.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
#include "sanitycheck.h"
#include "manualcontrol.h"
#include "manualcontrolsettings.h"
#include "manualcontrolcommand.h"
#include "positionstate.h"
#include "pathdesired.h"
#include "stabilizationsettings.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,101 @@
#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
// Private types
typedef enum {
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// defined handlers
static const controlHandler handler_MANUAL = {
.controlChain = {
.Stabilization = false,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &manualHandler,
};
static const controlHandler handler_STABILIZED = {
.controlChain = {
.Stabilization = true,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &stabilizedHandler,
};
static const controlHandler handler_AUTOTUNE = {
.controlChain = {
.Stabilization = false,
.PathFollower = false,
.PathPlanner = false,
},
.handler = NULL,
};
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
// TODO: move the altitude handling into stabi
static const controlHandler handler_ALTITUDE = {
.controlChain = {
.Stabilization = true,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &altitudeHandler,
};
static const controlHandler handler_PATHFOLLOWER = {
.controlChain = {
.Stabilization = true,
.PathFollower = true,
.PathPlanner = false,
},
.handler = &pathFollowerHandler,
};
static const controlHandler handler_PATHPLANNER = {
.controlChain = {
.Stabilization = true,
.PathFollower = true,
.PathPlanner = true,
},
.handler = &pathPlannerHandler,
};
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
// 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);
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 +156,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,1105 +173,77 @@ 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);
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);
}
// 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);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&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.25f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// Stop updating AltitudeHoldDesired triggering a failsafe condition.
if (cmd->Throttle < 0) {
return;
}
// this is the max speed in m/s at the extents of throttle
uint8_t throttleRate;
uint8_t throttleExp;
static uint8_t flightMode;
static portTickType lastSysTimeAH;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
FlightStatusFlightModeGet(&flightMode);
AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
throttleExp = 128;
throttleRate = 0;
} else {
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
}
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f);
lastSysTimeAH = thisSysTime;
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
float currentDown;
PositionStateDownGet(&currentDown);
if (changed) {
// After not being in this mode for a while init at current height
altitudeHoldDesiredData.Altitude = 0;
zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
// 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 + x*(256−k)) / 256
altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) {
// Require the stick to enter the dead band before they can move height
// Vario is not "engaged" when throttleRate == 0
zeroed = true;
}
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
const 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:
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
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;
#endif
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)
{
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;
}
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
*/
@ -1272,6 +252,14 @@ static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
configuration_check();
}
/**
* Called whenever a critical configuration component changes
*/
static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
}
/**
* @}
* @}

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

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

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

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

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

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

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

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

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

@ -63,8 +63,6 @@
// Private types
// Private variables
static uint32_t idleCounter;
static uint32_t idleCounterClear;
static xTaskHandle systemTaskHandle;
static bool stackOverflow;
static bool mallocFailed;
@ -119,7 +117,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,
@ -130,8 +128,6 @@ static void systemTask(__attribute__((unused)) void *parameters)
}
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
lastSysTime = xTaskGetTickCount();
// Main system loop
@ -205,15 +201,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
* Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
*/
void vApplicationIdleHook(void)
{
// Called when the scheduler has no tasks to run
if (idleCounterClear == 0) {
++idleCounter;
} else {
idleCounter = 0;
idleCounterClear = 0;
}
}
{}
/**
* Called by the RTOS when a stack overflow is detected.

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

@ -31,6 +31,8 @@
#include "openpilot.h"
#include "callbackinfo.h"
#include "pathplan.h"
#include "flightstatus.h"
#include "airspeedstate.h"
#include "pathaction.h"
@ -40,23 +42,26 @@
#include "velocitystate.h"
#include "waypoint.h"
#include "waypointactive.h"
#include "taskinfo.h"
#include "flightmodesettings.h"
#include <pios_struct_helper.h>
#include "paths.h"
// Private constants
#define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define TASK_PRIORITY CALLBACK_TASK_NAVIGATION
#define MAX_QUEUE_SIZE 2
#define PATH_PLANNER_UPDATE_RATE_MS 20
#define PATH_PLANNER_UPDATE_RATE_MS 100 // can be slow, since we listen to status updates as well
// Private types
// Private functions
static void pathPlannerTask(void *parameters);
static void updatePathDesired(UAVObjEvent *ev);
static void pathPlannerTask();
static void commandUpdated(UAVObjEvent *ev);
static void statusUpdated(UAVObjEvent *ev);
static void updatePathDesired();
static void setWaypoint(uint16_t num);
static uint8_t checkPathPlan();
static uint8_t pathConditionCheck();
static uint8_t conditionNone();
static uint8_t conditionTimeOut();
@ -71,7 +76,8 @@ static uint8_t conditionImmediate();
// Private variables
static xTaskHandle taskHandle;
static DelayedCallbackInfo *pathPlannerHandle;
static DelayedCallbackInfo *pathDesiredUpdaterHandle;
static WaypointActiveData waypointActive;
static WaypointData waypoint;
static PathActionData pathAction;
@ -83,11 +89,14 @@ static bool pathplanner_active = false;
*/
int32_t PathPlannerStart()
{
taskHandle = NULL;
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(commandUpdated);
WaypointActiveConnectCallback(commandUpdated);
PathActionConnectCallback(commandUpdated);
PathStatusConnectCallback(statusUpdated);
// Start VM thread
xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHPLANNER, taskHandle);
// Start main task callback
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
return 0;
}
@ -97,8 +106,7 @@ int32_t PathPlannerStart()
*/
int32_t PathPlannerInitialize()
{
taskHandle = NULL;
PathPlanInitialize();
PathActionInitialize();
PathStatusInitialize();
PathDesiredInitialize();
@ -108,6 +116,9 @@ int32_t PathPlannerInitialize()
WaypointInitialize();
WaypointActiveInitialize();
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;
}
@ -116,129 +127,250 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
/**
* Module task
*/
static void pathPlannerTask(__attribute__((unused)) void *parameters)
static void pathPlannerTask()
{
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(updatePathDesired);
WaypointActiveConnectCallback(updatePathDesired);
PathActionConnectCallback(updatePathDesired);
PIOS_CALLBACKSCHEDULER_Schedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
bool endCondition = false;
// check path plan validity early to raise alarm
// even if not in guided mode
uint8_t validPathPlan = checkPathPlan();
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
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
// so it does not prevent arming. However manualcontrols safety check
// shall test for this warning when pathplan is on the flight mode selector
// thus a valid flight plan is a prerequirement for arming
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
}
return;
}
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
static uint8_t failsafeRTHset = 0;
if (!validPathPlan) {
// At this point the craft is in PathPlanner mode, so pilot has no manual control capability.
// Failsafe: behave as if in return to base mode
// what to do in this case is debatable, it might be better to just
// make a forced disarming but rth has a higher chance of survival when
// in flight.
pathplanner_active = false;
if (!failsafeRTHset) {
failsafeRTHset = 1;
// copy pasta: same calculation as in manualcontrol, set return to home coordinates
PositionStateData positionState;
PositionStateGet(&positionState);
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
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);
}
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
return;
}
failsafeRTHset = 0;
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
WaypointActiveGet(&waypointActive);
if (pathplanner_active == false) {
pathplanner_active = true;
// This triggers callback to update variable
waypointActive.Index = 0;
WaypointActiveSet(&waypointActive);
return;
}
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
PathStatusData pathStatus;
PathStatusGet(&pathStatus);
// Main thread loop
bool endCondition = false;
while (1) {
vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS);
// delay next step until path follower has acknowledged the path mode
if (pathStatus.UID != pathDesired.UID) {
return;
}
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
pathplanner_active = false;
continue;
// negative destinations DISABLE this feature
if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
setWaypoint(pathAction.ErrorDestination);
return;
}
// check if condition has been met
endCondition = pathConditionCheck();
// decide what to do
switch (pathAction.Command) {
case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
if (endCondition) {
setWaypoint(waypointActive.Index + 1);
}
WaypointActiveGet(&waypointActive);
if (pathplanner_active == false) {
pathplanner_active = true;
// This triggers callback to update variable
waypointActive.Index = 0;
WaypointActiveSet(&waypointActive);
continue;
}
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
PathStatusGet(&pathStatus);
PathDesiredGet(&pathDesired);
// delay next step until path follower has acknowledged the path mode
if (pathStatus.UID != pathDesired.UID) {
continue;
}
// negative destinations DISABLE this feature
if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
setWaypoint(pathAction.ErrorDestination);
continue;
}
// check if condition has been met
endCondition = pathConditionCheck();
// decide what to do
switch (pathAction.Command) {
case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
if (endCondition) {
setWaypoint(waypointActive.Index + 1);
}
break;
case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination < 0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination);
} else {
setWaypoint(pathAction.JumpDestination);
}
}
break;
case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination < 0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination);
} else {
setWaypoint(pathAction.JumpDestination);
}
break;
case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination < 0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination);
} else {
setWaypoint(waypointActive.Index + 1);
setWaypoint(pathAction.JumpDestination);
}
break;
}
break;
case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination < 0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination);
} else {
setWaypoint(pathAction.JumpDestination);
}
} else {
setWaypoint(waypointActive.Index + 1);
}
break;
}
}
// safety checks for path plan integrity
static uint8_t checkPathPlan()
{
uint16_t i;
uint16_t waypointCount;
uint16_t actionCount;
uint8_t pathCrc;
PathPlanData pathPlan;
// WaypointData waypoint; // using global instead (?)
// PathActionData action; // using global instead (?)
PathPlanGet(&pathPlan);
waypointCount = pathPlan.WaypointCount;
if (waypointCount == 0) {
// an empty path plan is invalid
return false;
}
actionCount = pathPlan.PathActionCount;
// check count consistency
if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) {
// PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!");
return false;
}
if (actionCount > UAVObjGetNumInstances(PathActionHandle())) {
// PIOS_DEBUGLOG_Printf("PathPlan : path action count error!");
return false;
}
// check CRC
pathCrc = 0;
for (i = 0; i < waypointCount; i++) {
pathCrc = UAVObjUpdateCRC(WaypointHandle(), i, pathCrc);
}
for (i = 0; i < actionCount; i++) {
pathCrc = UAVObjUpdateCRC(PathActionHandle(), i, pathCrc);
}
if (pathCrc != pathPlan.Crc) {
// failed crc check
// PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc);
return false;
}
// waypoint consistency
for (i = 0; i < waypointCount; i++) {
WaypointInstGet(i, &waypoint);
if (waypoint.Action >= actionCount) {
// path action id is out of range
return false;
}
}
// path action consistency
for (i = 0; i < actionCount; i++) {
PathActionInstGet(i, &pathAction);
if (pathAction.ErrorDestination >= waypointCount) {
// waypoint id is out of range
return false;
}
if (pathAction.JumpDestination >= waypointCount) {
// waypoint id is out of range
return false;
}
}
// path plan passed checks
return true;
}
// callback function when status changed, issue execution of state machine
void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(pathDesiredUpdaterHandle);
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired()
{
// only ever touch pathDesired if pathplanner is enabled
if (!pathplanner_active) {
return;
}
// use local variables, dont use stack since this is huge and a callback,
// dont use the globals because we cant use mutexes here
static WaypointActiveData waypointActiveData;
static PathActionData pathActionData;
static WaypointData waypointData;
static PathDesiredData pathDesired;
PathDesiredData pathDesired;
// find out current waypoint
WaypointActiveGet(&waypointActiveData);
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActiveData.Index, &waypointData);
PathActionInstGet(waypointData.Action, &pathActionData);
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
pathDesired.End.North = waypointData.Position.North;
pathDesired.End.East = waypointData.Position.East;
pathDesired.End.Down = waypointData.Position.Down;
pathDesired.EndingVelocity = waypointData.Velocity;
pathDesired.Mode = pathActionData.Mode;
pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0];
pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1];
pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2];
pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3];
pathDesired.UID = waypointActiveData.Index;
pathDesired.End.North = waypoint.Position.North;
pathDesired.End.East = waypoint.Position.East;
pathDesired.End.Down = waypoint.Position.Down;
pathDesired.EndingVelocity = waypoint.Velocity;
pathDesired.Mode = pathAction.Mode;
pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
pathDesired.UID = waypointActive.Index;
if (waypointActiveData.Index == 0) {
if (waypointActive.Index == 0) {
PositionStateData positionState;
PositionStateGet(&positionState);
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
@ -266,8 +398,13 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
// helper function to go to a specific waypoint
static void setWaypoint(uint16_t num)
{
// path plans wrap around
if (num >= UAVObjGetNumInstances(WaypointHandle())) {
PathPlanData pathPlan;
PathPlanGet(&pathPlan);
// here it is assumed that the path plan has been validated (waypoint count is consistent)
if (num >= pathPlan.WaypointCount) {
// path plans wrap around
num = 0;
}
@ -275,10 +412,10 @@ static void setWaypoint(uint16_t num)
WaypointActiveSet(&waypointActive);
}
// execute the apropriate condition and report result
// execute the appropriate condition and report result
static uint8_t pathConditionCheck()
{
// i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's
// i thought about a lookup table, but a switch is safer considering there could be invalid EndCondition ID's
switch (pathAction.EndCondition) {
case PATHACTION_ENDCONDITION_NONE:
return conditionNone();

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

@ -1,5 +1,6 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module
@ -36,6 +37,7 @@
#include <objectpersistence.h>
#include <oplinksettings.h>
#include <oplinkreceiver.h>
#include <radiocombridgestats.h>
#include <uavtalk_priv.h>
#include <pios_rfm22b.h>
#include <ecc.h>
@ -57,6 +59,7 @@
#define SERIAL_RX_BUF_LEN 100
#define PPM_INPUT_TIMEOUT 100
// ****************
// Private types
@ -81,10 +84,11 @@ typedef struct {
uint8_t serialRxBuf[SERIAL_RX_BUF_LEN];
// Error statistics.
uint32_t comTxErrors;
uint32_t comTxRetries;
uint32_t UAVTalkErrors;
uint32_t droppedPackets;
uint32_t telemetryTxRetries;
uint32_t radioTxRetries;
// Is this modem the coordinator
bool isCoordinator;
// Should we parse UAVTalk?
bool parseUAVTalk;
@ -107,6 +111,7 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
static void objectPersistenceUpdatedCb(UAVObjEvent *objEv);
static void registerObject(UAVObjHandle obj);
// ****************
// Private variables
@ -124,12 +129,14 @@ static int32_t RadioComBridgeStart(void)
// Get the settings.
OPLinkSettingsData oplinkSettings;
OPLinkSettingsGet(&oplinkSettings);
bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
// Check if this is the coordinator modem
data->isCoordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
// We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port).
data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
(oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
(oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
(oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
(oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
// Set the maximum radio RF power.
switch (oplinkSettings.MaxRFPower) {
@ -165,12 +172,16 @@ static int32_t RadioComBridgeStart(void)
// Configure our UAVObjects for updates.
UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
if (is_coordinator) {
if (data->isCoordinator) {
UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->radioEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
} else {
UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
}
if (data->isCoordinator) {
registerObject(RadioComBridgeStatsHandle());
}
// Configure the UAVObject callbacks
ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb);
@ -223,27 +234,94 @@ static int32_t RadioComBridgeInitialize(void)
OPLinkStatusInitialize();
ObjectPersistenceInitialize();
OPLinkReceiverInitialize();
RadioComBridgeStatsInitialize();
// Initialise UAVTalk
data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler);
data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler);
data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler);
data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler);
// Initialize the queues.
data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
// Initialize the statistics.
data->comTxErrors = 0;
data->comTxRetries = 0;
data->UAVTalkErrors = 0;
data->parseUAVTalk = true;
data->comSpeed = OPLINKSETTINGS_COMSPEED_9600;
PIOS_COM_RADIO = PIOS_COM_RFM22B;
data->telemetryTxRetries = 0;
data->radioTxRetries = 0;
data->parseUAVTalk = true;
data->comSpeed = OPLINKSETTINGS_COMSPEED_9600;
PIOS_COM_RADIO = PIOS_COM_RFM22B;
return 0;
}
MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart);
// TODO this code (badly) duplicates code from telemetry.c
// This method should be used only for periodically updated objects.
// The register method defined in telemetry.c should be factored out in a shared location so it can be
// used from here...
static void registerObject(UAVObjHandle obj)
{
// Setup object for periodic updates
UAVObjEvent ev = {
.obj = obj,
.instId = UAVOBJ_ALL_INSTANCES,
.event = EV_UPDATED_PERIODIC,
.lowPriority = true,
};
// Get metadata
UAVObjMetadata metadata;
UAVObjGetMetadata(obj, &metadata);
EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod);
UAVObjConnectQueue(obj, data->uavtalkEventQueue, EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
}
/**
* Update telemetry statistics
*/
static void updateRadioComBridgeStats()
{
UAVTalkStats telemetryUAVTalkStats;
UAVTalkStats radioUAVTalkStats;
RadioComBridgeStatsData radioComBridgeStats;
// Get telemetry stats
UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats, true);
// Get radio stats
UAVTalkGetStats(data->radioUAVTalkCon, &radioUAVTalkStats, true);
// Get stats object data
RadioComBridgeStatsGet(&radioComBridgeStats);
radioComBridgeStats.TelemetryTxRetries = data->telemetryTxRetries;
radioComBridgeStats.RadioTxRetries = data->radioTxRetries;
// Update stats object
radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes;
radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors;
radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes;
radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors;
radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors;
radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors;
radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes;
radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors;
radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes;
radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors;
radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors;
radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors;
// Update stats object data
RadioComBridgeStatsSet(&radioComBridgeStats);
}
/**
* @brief Telemetry transmit task, regular priority
*
@ -260,18 +338,20 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
#endif
// Wait for queue message
if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) {
// Send update (with retries)
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0;
if (!success) {
++retries;
}
}
data->comTxRetries += retries;
if (ev.obj == RadioComBridgeStatsHandle()) {
updateRadioComBridgeStats();
}
// Send update (with retries)
int32_t ret = -1;
uint32_t retries = 0;
while (retries <= MAX_RETRIES && ret == -1) {
ret = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS);
if (ret == -1) {
++retries;
}
}
// Update stats
data->telemetryTxRetries += retries;
}
}
}
@ -296,15 +376,15 @@ static void radioTxTask(__attribute__((unused)) void *parameters)
if (xQueueReceive(data->radioEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) {
// Send update (with retries)
int32_t ret = -1;
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0;
if (!success) {
while (retries <= MAX_RETRIES && ret == -1) {
ret = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS);
if (ret == -1) {
++retries;
}
}
data->comTxRetries += retries;
data->radioTxRetries += retries;
}
}
}
@ -333,7 +413,13 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
}
} else if (PIOS_COM_TELEMETRY) {
// Send the data straight to the telemetry port.
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
// Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
// It is the caller responsibility to retry in such cases...
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 {
@ -425,9 +511,15 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
// Receive some data.
uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, data->serialRxBuf, sizeof(data->serialRxBuf), MAX_PORT_DELAY);
// Send the data over the radio link.
if (bytes_to_process > 0) {
PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
// Send the data over the radio link.
// Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
// It is the caller responsibility to retry in such cases...
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);
@ -445,6 +537,7 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
*/
static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
{
int32_t ret;
uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0;
#if defined(PIOS_INCLUDE_USB)
@ -454,10 +547,17 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
}
#endif /* PIOS_INCLUDE_USB */
if (outputPort) {
return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
// Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
// It is the caller responsibility to retry in such cases...
ret = -2;
uint8_t count = 5;
while (count-- > 0 && ret < -1) {
ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
}
} else {
return -1;
ret = -1;
}
return ret;
}
/**
@ -477,10 +577,16 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
// Don't send any data unless the radio port is available.
if (outputPort && PIOS_COM_Available(outputPort)) {
return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
// Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
// It is the caller responsibility to retry in such cases...
int32_t ret = -2;
uint8_t count = 5;
while (count-- > 0 && ret < -1) {
ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
}
return ret;
} else {
// For some reason, if this function returns failure, it prevents saving settings.
return length;
return -1;
}
}
@ -494,12 +600,40 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
{
// Keep reading until we receive a completed packet.
UAVTalkRxState state = UAVTalkProcessInputStream(inConnectionHandle, rxbyte);
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
if (state == UAVTALK_STATE_ERROR) {
data->UAVTalkErrors++;
} else if (state == UAVTALK_STATE_COMPLETE) {
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
if (state == UAVTALK_STATE_COMPLETE) {
// We only want to unpack certain telemetry objects
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
switch (objId) {
case OPLINKSTATUS_OBJID:
case OPLINKSETTINGS_OBJID:
case OPLINKRECEIVER_OBJID:
case MetaObjectId(OPLINKSTATUS_OBJID):
case MetaObjectId(OPLINKSETTINGS_OBJID):
case MetaObjectId(OPLINKRECEIVER_OBJID):
UAVTalkReceiveObject(inConnectionHandle);
break;
case OBJECTPERSISTENCE_OBJID:
case MetaObjectId(OBJECTPERSISTENCE_OBJID):
// receive object locally
// some objects will send back a response to telemetry
// FIXME:
// OPLM will ack or nack all objects requests and acked object sends
// Receiver will probably also ack / nack the same messages
// This has some consequences like :
// Second ack/nack will not match an open transaction or will apply to wrong transaction
// Question : how does GCS handle receiving the same object twice
// The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
UAVTalkReceiveObject(inConnectionHandle);
// relay packet to remote modem
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
default:
// all other packets are relayed to the remote modem
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
}
}
}
@ -515,19 +649,30 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn
// Keep reading until we receive a completed packet.
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
if (state == UAVTALK_STATE_ERROR) {
data->UAVTalkErrors++;
} else if (state == UAVTALK_STATE_COMPLETE) {
// We only want to unpack certain objects from the remote modem.
if (state == UAVTALK_STATE_COMPLETE) {
// We only want to unpack certain objects from the remote modem
// Similarly we only want to relay certain objects to the telemetry port
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
switch (objId) {
case OPLINKSTATUS_OBJID:
case OPLINKSETTINGS_OBJID:
case MetaObjectId(OPLINKSTATUS_OBJID):
case MetaObjectId(OPLINKSETTINGS_OBJID):
// Ignore object...
// These objects are shadowed by the modem and are not transmitted to the telemetry port
// - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
// - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
break;
case OPLINKRECEIVER_OBJID:
case MetaObjectId(OPLINKRECEIVER_OBJID):
// Receive object locally
// These objects are received by the modem and are not transmitted to the telemetry port
// - OPLINKRECEIVER_OBJID : not sure why
// some objects will send back a response to the remote modem
UAVTalkReceiveObject(inConnectionHandle);
break;
default:
// all other packets are relayed to the telemetry port
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
}
@ -545,7 +690,7 @@ static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objE
ObjectPersistenceGet(&obj_per);
// Is this concerning or setting object?
// Is this concerning our setting object?
if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) {
// Is this a save, load, or delete?
bool success = false;

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

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

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

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

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

@ -35,6 +35,10 @@
#include <pios_struct_helper.h>
#include "stabilization.h"
#include "stabilizationsettings.h"
#include "stabilizationbank.h"
#include "stabilizationsettingsbank1.h"
#include "stabilizationsettingsbank2.h"
#include "stabilizationsettingsbank3.h"
#include "actuatordesired.h"
#include "ratedesired.h"
#include "relaytuning.h"
@ -44,7 +48,9 @@
#include "airspeedstate.h"
#include "gyrostate.h"
#include "flightstatus.h"
#include "manualcontrol.h" // Just to get a macro
#include "manualcontrolsettings.h"
#include "manualcontrolcommand.h"
#include "flightmodesettings.h"
#include "taskinfo.h"
// Math libraries
@ -60,22 +66,27 @@
#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 724
#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
// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode
// The PID_RATE set is used by the attitude portion of Attitude mode
// The PID_RATEA_ROLL set is used by Rattitude mode because it needs to maintain
// - two independant rate PIDs because it does rate and attitude simultaneously
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_RATEA_ROLL, PID_RATEA_PITCH, PID_RATEA_YAW, PID_MAX };
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX };
enum { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET };
enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET };
// Private variables
static xTaskHandle taskHandle;
@ -88,18 +99,28 @@ 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;
float vbar_decay = 0.991f;
struct pid pids[PID_MAX];
static uint8_t rattitude_anti_windup;
int cur_flight_mode = -1;
static float rattitude_mode_transition_stick_position;
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;
static int8_t cruise_control_inverted_power_switch;
static float cruise_control_neutral_thrust;
static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
// Private functions
static void stabilizationTask(void *parameters);
static float bound(float val, float range);
static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static float stab_log2f(float x);
static float stab_powf(float x, uint8_t p);
static void BankUpdatedCb(UAVObjEvent *ev);
static void SettingsBankUpdatedCb(UAVObjEvent *ev);
/**
* Module initialization
@ -117,6 +138,13 @@ int32_t StabilizationStart()
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
StabilizationBankConnectCallback(BankUpdatedCb);
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
// Start main task
xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle);
@ -132,7 +160,15 @@ int32_t StabilizationStart()
int32_t StabilizationInitialize()
{
// Initialize variables
ManualControlCommandInitialize();
ManualControlSettingsInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
StabilizationSettingsInitialize();
StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
StabilizationSettingsBank2Initialize();
StabilizationSettingsBank3Initialize();
ActuatorDesiredInitialize();
#ifdef DIAG_RATEDESIRED
RateDesiredInitialize();
@ -156,15 +192,19 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
static void stabilizationTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
PiOSDeltatimeConfig timeval;
uint32_t timeval = PIOS_DELAY_GetRaw();
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
float throttleDesired;
RateDesiredData rateDesired;
AttitudeStateData attitudeState;
GyroStateData gyroStateData;
FlightStatusData flightStatus;
StabilizationBankData stabBank;
#ifdef REVOLUTION
AirspeedStateData airspeedState;
@ -187,16 +227,24 @@ 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);
#ifdef DIAG_RATEDESIRED
RateDesiredGet(&rateDesired);
#endif
uint8_t flight_mode_switch_position;
ManualControlCommandFlightModeSwitchPositionGet(&flight_mode_switch_position);
if (cur_flight_mode != flight_mode_switch_position) {
cur_flight_mode = flight_mode_switch_position;
SettingsBankUpdatedCb(NULL);
}
#ifdef REVOLUTION
float speedScaleFactor;
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
@ -294,7 +342,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] =
bound(stabDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
bound(stabDesiredAxis[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
@ -311,7 +359,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]);
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
@ -329,14 +377,13 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
if (reinit) {
pids[PID_ROLL + i].iAccumulator = 0;
pids[PID_RATE_ROLL + i].iAccumulator = 0;
pids[PID_RATEA_ROLL + i].iAccumulator = 0;
}
// Compute what Rate mode would give for this stick angle's rate
// Save Rate's rate in a temp for later merging with Attitude's rate
float rateDesiredAxisRate;
rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f)
* cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i];
* cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i];
// Compute what Attitude mode would give for this stick angle's rate
@ -345,15 +392,15 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// - subtract off the actual angle to get the angle error
// This is what local_error[] holds for Attitude mode
float attitude_error = stabDesiredAxis[i]
* cast_struct_to_array(settings.RollMax, settings.RollMax)[i]
* cast_struct_to_array(stabBank.RollMax, stabBank.RollMax)[i]
- cast_struct_to_array(attitudeState.Roll, attitudeState.Roll)[i];
// Compute the outer loop just like Attitude mode does
float rateDesiredAxisAttitude;
rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT);
rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude,
cast_struct_to_array(settings.MaximumRate,
settings.MaximumRate.Roll)[i]);
cast_struct_to_array(stabBank.ManualRate,
stabBank.ManualRate.Roll)[i]);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
@ -363,81 +410,54 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch);
float magnitude;
magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch));
// modify magnitude to move the Att to Rate transition to the place
// specified by the user
// we are looking for where the stick angle == transition angle
// and the Att rate equals the Rate rate
// that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
// == Rate x StickAngle [Rate pulling up according to stick angle]
// * StickAngle [X Ratt proportion]
// so 1-x == x*x or x*x+x-1=0 where xE(0,1)
// (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
// and quadratic formula says that is 0.618033989f
// I tested 14.01 and came up with .61 without even remembering this number
// I thought that moving the P,I, and maxangle terms around would change this value
// and that I would have to take these into account, but varying
// all P's and I's by factors of 1/2 to 2 didn't change it noticeably
// and varying maxangle from 4 to 120 didn't either.
// so for now I'm not taking these into account
// while working with this, it occurred to me that Attitude mode,
// set up with maxangle=190 would be similar to Ratt, and it is.
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
// the following assumes the transition would otherwise be at 0.618033989f
// and THAT assumes that Att ramps up to max roll rate
// when a small number of degrees off of where it should be
// if below the transition angle (still in attitude mode)
// '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
if (magnitude <= rattitude_mode_transition_stick_position) {
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position;
} else {
magnitude = (magnitude - rattitude_mode_transition_stick_position)
* (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
/ (1.0f - rattitude_mode_transition_stick_position)
+ STICK_VALUE_AT_MODE_TRANSITION;
}
rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
+ magnitude * rateDesiredAxisRate;
// Compute the inner loop for both Rate mode and Attitude mode
// actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates
actuatorDesiredAxis[i] =
(1.0f - magnitude) * pid_apply_setpoint(&pids[PID_RATEA_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT)
+ magnitude * pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
// Compute the inner loop for the averaged rate
// actuatorDesiredAxis[i] is the weighted average
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor,
rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
// settings.RattitudeAntiWindup controls the iAccumulator zeroing
// - so both iAccs don't wind up too far;
// - nor do both iAccs get held too close to zero at mid stick
// I suspect that there would be windup without it
// - since rate and attitude fight each other here
// - rate trying to pull it over the top and attitude trying to pull it back down
// Wind-up increases linearly with cycles for a fixed error.
// We must never increase the iAcc or we risk oscillation.
// Use the powf() function to make two anti-windup curves
// - one for zeroing rate close to center stick
// - the other for zeroing attitude close to max stick
// the bigger the dT the more anti windup needed
// the bigger the PID[].i the more anti windup needed
// more anti windup is achieved with a lower powf() power
// a doubling of e.g. PID[].i should cause the runtime anti windup factor
// to get twice as far away from 1.0 (towards zero)
// e.g. from .90 to .80
// magic numbers
// these generate the inverted parabola like curves that go through [0,1] and [1,0]
// the higher the power, the closer the curve is to a straight line
// from [0,1] to [1,1] to [1,0] and the less anti windup is applied
// the UAVO RattitudeAntiWindup varies from 0 to 31
// 0 turns off anti windup
// 1 gives very little anti-windup because the power given to powf() is 31
// 31 gives a lot of anti-windup because the power given to powf() is 1
// the 32.1 is what does this
// the 7.966 and 17.668 cancel the default PID value and dT given to log2f()
// if these are non-default, tweaking is thus done so the user doesn't have to readjust
// the default value of 10 for UAVO RattitudeAntiWindup gives a power of 22
// these calculations are for magnitude = 0.5, so 22 corresponds to the number of bits
// used in the mantissa of the float
// i.e. 1.0-(0.5^22) almost underflows
// This may only be useful for aircraft with large Ki values and limits
if (dT > 0.0f && rattitude_anti_windup > 0.0f) {
float factor;
// At magnitudes close to one, the Attitude accumulators gets zeroed
if (pids[PID_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 7.966f - stab_log2f(dT * pids[PID_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_ROLL + i].iAccumulator *= factor;
}
if (pids[PID_RATEA_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_RATEA_ROLL + i].iAccumulator *= factor;
}
// At magnitudes close to zero, the Rate accumulator gets zeroed
if (pids[PID_RATE_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(1.0f - magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_RATE_ROLL + i].iAccumulator *= factor;
}
}
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
// Store for debugging output
rateDesiredAxis[i] = stabDesiredAxis[i];
@ -486,7 +506,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
@ -496,7 +516,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(stabDesiredAxis[i],
cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
@ -512,7 +532,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute the outer loop like attitude mode
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]);
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
@ -539,24 +559,66 @@ 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;
// modify thrust according to 1/cos(bank angle)
// to maintain same altitdue with changing bank angle
// 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 < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
&& cruise_control_max_power_factor > 0.0001f) {
static uint8_t toggle;
static float factor;
float angle;
// get attitude state and calculate angle
// do it every 8th iteration to save CPU
if ((toggle++ & 7) == 0) {
// spherical right triangle
// 0 <= acosf() <= Pi
angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch)));
// if past the cutoff angle (60 to 180 (180 means never))
if (angle > cruise_control_max_angle) {
// -1 reversed collective, 0 zero power, or 1 normal power
// these are all unboosted
factor = cruise_control_inverted_power_switch;
} else {
// avoid singularity
if (angle > 89.999f && angle < 90.001f) {
factor = cruise_control_max_power_factor;
} else {
factor = 1.0f / fabsf(cos_lookup_deg(angle));
if (factor > cruise_control_max_power_factor) {
factor = cruise_control_max_power_factor;
}
}
// factor in the power trim, no effect at 1.0, linear effect increases with factor
factor = (factor - 1.0f) * cruise_control_power_trim + 1.0f;
// if inverted and they want negative boost
if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t)-1) {
factor = -factor;
// 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;
}
}
}
if (lowThrottleZeroAxis[PITCH]) {
actuatorDesired.Pitch = 0.0f;
}
if (lowThrottleZeroAxis[YAW]) {
actuatorDesired.Yaw = 0.0f;
// 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.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
@ -566,7 +628,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;
@ -606,46 +668,122 @@ static void ZeroPids(void)
static float bound(float val, float range)
{
if (val < -range) {
val = -range;
return -range;
} else if (val > range) {
val = range;
return range;
}
return val;
}
// x small (0.0 < x < .01) so interpolation of fractional part is reasonable
static float stab_log2f(float x)
static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
union {
volatile float f;
volatile uint32_t i;
volatile unsigned char c[4];
} __attribute__((packed)) u1, u2;
u2.f = u1.f = x;
u1.i <<= 1;
u2.i &= 0xff800000;
// get and unbias the exponent, add in a linear interpolation of the fractional part
return (float)(u1.c[3] - 127) + (x / u2.f) - 1.0f;
}
// 0<=x<=1, 0<=p<=31
static float stab_powf(float x, uint8_t p)
{
float retval = 1.0f;
while (p) {
if (p & 1) {
retval *= x;
}
x *= x;
p >>= 1;
if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
return;
}
if ((ev) && ((settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
(settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
(settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
settings.FlightModeMap[cur_flight_mode] > 2)) {
return;
}
return retval;
StabilizationBankData bank;
switch (settings.FlightModeMap[cur_flight_mode]) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
break;
}
StabilizationBankSet(&bank);
}
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationBankData bank;
StabilizationBankGet(&bank);
// this code will be needed if any other modules alter stabilizationbank
/*
StabilizationBankData curBank;
if(flight_mode < 0) return;
switch(cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode])
{
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *) &bank);
}
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *) &bank);
}
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *) &bank);
}
break;
default:
return; //invalid bank number
}
*/
// Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], bank.RollRatePID.Kp,
bank.RollRatePID.Ki,
bank.RollRatePID.Kd,
bank.RollRatePID.ILimit);
// Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH], bank.PitchRatePID.Kp,
bank.PitchRatePID.Ki,
bank.PitchRatePID.Kd,
bank.PitchRatePID.ILimit);
// Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW], bank.YawRatePID.Kp,
bank.YawRatePID.Ki,
bank.YawRatePID.Kd,
bank.YawRatePID.ILimit);
// Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], bank.RollPI.Kp,
bank.RollPI.Ki,
0,
bank.RollPI.ILimit);
// Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], bank.PitchPI.Kp,
bank.PitchPI.Ki,
0,
bank.PitchPI.ILimit);
// Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], bank.YawPI.Kp,
bank.YawPI.Ki,
0,
bank.YawPI.ILimit);
}
@ -653,69 +791,6 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationSettingsGet(&settings);
// Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL],
settings.RollRatePID.Kp,
settings.RollRatePID.Ki,
settings.RollRatePID.Kd,
settings.RollRatePID.ILimit);
// Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH],
settings.PitchRatePID.Kp,
settings.PitchRatePID.Ki,
settings.PitchRatePID.Kd,
settings.PitchRatePID.ILimit);
// Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW],
settings.YawRatePID.Kp,
settings.YawRatePID.Ki,
settings.YawRatePID.Kd,
settings.YawRatePID.ILimit);
// Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL],
settings.RollPI.Kp,
settings.RollPI.Ki,
0,
settings.RollPI.ILimit);
// Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH],
settings.PitchPI.Kp,
settings.PitchPI.Ki,
0,
settings.PitchPI.ILimit);
// Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW],
settings.YawPI.Kp,
settings.YawPI.Ki,
0,
settings.YawPI.ILimit);
// Set the Rattitude roll rate PID constants
pid_configure(&pids[PID_RATEA_ROLL],
settings.RollRatePID.Kp,
settings.RollRatePID.Ki,
settings.RollRatePID.Kd,
settings.RollRatePID.ILimit);
// Set the Rattitude pitch rate PID constants
pid_configure(&pids[PID_RATEA_PITCH],
settings.PitchRatePID.Kp,
settings.PitchRatePID.Ki,
settings.PitchRatePID.Kd,
settings.PitchRatePID.ILimit);
// Set the Rattitude yaw rate PID constants
pid_configure(&pids[PID_RATEA_YAW],
settings.YawRatePID.Kp,
settings.YawRatePID.Ki,
settings.YawRatePID.Kd,
settings.YawRatePID.ILimit);
// Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
@ -727,13 +802,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
@ -750,10 +820,29 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
// Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau);
// Rattitude flight mode anti-windup factor
rattitude_anti_windup = settings.RattitudeAntiWindup;
}
// force flight mode update
cur_flight_mode = -1;
// Rattitude stick angle where the attitude to rate transition happens
if (settings.RattitudeModeTransition < (uint8_t)10) {
rattitude_mode_transition_stick_position = 10.0f / 100.0f;
} else {
rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 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;
cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
cruise_control_neutral_thrust = (float)settings.CruiseControlNeutralThrust / 100.0f;
memcpy(
cruise_control_flight_mode_switch_pos_enable,
settings.CruiseControlFlightModeSwitchPosEnable,
sizeof(cruise_control_flight_mode_switch_pos_enable));
}
/**
* @}

2
flight/modules/StateEstimation/filterair.c
View File

@ -78,7 +78,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->altitude = state->baro[0];
}
// calculate true airspeed estimation
if (IS_SET(state->updated, SENSORUPDATES_airspeed)) {
if (IS_SET(state->updated, SENSORUPDATES_airspeed) && (state->airspeed[1] < 0.f)) {
state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude);
}

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

@ -38,22 +38,32 @@
// Private constants
#define STACK_REQUIRED 128
// duration of accel bias initialization phase
#define INITIALIZATION_DURATION_MS 5000
#define DT_ALPHA 1e-3f
#define STACK_REQUIRED 128
#define DT_ALPHA 1e-2f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_AVERAGE 1e-3f
static volatile bool reloadSettings;
// 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 altitudeState; // state = altitude,velocity,accel_offset,accel
float velocityState;
float accelBiasState;
float accelState;
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;
portTickType initTimer;
AltitudeFilterSettingsData settings;
};
@ -63,6 +73,7 @@ struct data {
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static void settingsUpdatedCb(UAVObjEvent *ev);
int32_t filterAltitudeInitialize(stateFilter *handle)
@ -72,6 +83,8 @@ int32_t filterAltitudeInitialize(stateFilter *handle)
handle->localdata = pvPortMalloc(sizeof(struct data));
AttitudeStateInitialize();
AltitudeFilterSettingsInitialize();
AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb);
reloadSettings = true;
return STACK_REQUIRED;
}
@ -79,22 +92,21 @@ static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->state[0] = 0.0f;
this->state[1] = 0.0f;
this->state[2] = 0.0f;
this->state[3] = 0.0f;
this->altitudeState = 0.0f;
this->velocityState = 0.0f;
this->accelBiasState = 0.0f;
this->accelState = 0.0f;
this->pos[0] = 0.0f;
this->pos[1] = 0.0f;
this->pos[2] = 0.0f;
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;
AltitudeFilterSettingsGet(&this->settings);
return 0;
}
@ -102,14 +114,16 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
if (reloadSettings) {
reloadSettings = false;
AltitudeFilterSettingsGet(&this->settings);
}
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;
this->initTimer = xTaskGetTickCount();
}
} else {
// save existing position and velocity updates so GPS will still work
@ -117,13 +131,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->pos[0] = state->pos[0];
this->pos[1] = state->pos[1];
this->pos[2] = state->pos[2];
state->pos[2] = -this->state[0];
state->pos[2] = -this->altitudeState;
}
if (IS_SET(state->updated, SENSORUPDATES_vel)) {
this->vel[0] = state->vel[0];
this->vel[1] = state->vel[1];
this->vel[2] = state->vel[2];
state->vel[2] = -this->state[1];
state->vel[2] = -this->velocityState;
}
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
// rotate accels into global coordinate frame
@ -134,68 +148,54 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f);
// low pass filter accelerometers
this->state[3] = (1.0f - this->settings.AccelLowPassKp) * this->state[3] + this->settings.AccelLowPassKp * current;
// correct accel offset (low pass zeroing)
this->state[2] = (1.0f - this->settings.AccelDriftKi) * this->state[2] + this->settings.AccelDriftKi * this->state[3];
this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current;
if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION_MS) {
// allow the offset to reach quickly the target value in case of small AccelDriftKi
this->accelBiasState = (1.0f - this->settings.InitializationAccelDriftKi) * this->accelBiasState + this->settings.InitializationAccelDriftKi * this->accelState;
} else {
// correct accel offset (low pass zeroing)
this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState;
}
// 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 speedLast = this->state[1];
//
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config);
float speedLast = this->velocityState;
this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * this->dTA;
this->accelLast = this->state[3] - this->state[2];
this->velocityState += 0.5f * (this->accelLast + (this->accelState - this->accelBiasState)) * dT;
this->accelLast = this->accelState - this->accelBiasState;
this->state[0] += 0.5f * (speedLast + this->state[1]) * this->dTA;
this->altitudeState += 0.5f * (speedLast + this->velocityState) * dT;
state->pos[0] = this->pos[0];
state->pos[1] = this->pos[1];
state->pos[2] = -this->state[0];
state->pos[2] = -this->altitudeState;
state->updated |= SENSORUPDATES_pos;
state->vel[0] = this->vel[0];
state->vel[1] = this->vel[1];
state->vel[2] = -this->state[1];
state->vel[2] = -this->velocityState;
state->updated |= SENSORUPDATES_vel;
}
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
// correct the altitude state (simple low pass)
this->state[0] = (1.0f - this->settings.BaroKp) * this->state[0] + this->settings.BaroKp * state->baro[0];
this->altitudeState = (1.0f - this->settings.BaroKp) * this->altitudeState + this->settings.BaroKp * state->baro[0];
// 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->velocityState = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->velocityState + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
this->baroLast = state->baro[0];
state->pos[0] = this->pos[0];
state->pos[1] = this->pos[1];
state->pos[2] = -this->state[0];
state->pos[2] = -this->altitudeState;
state->updated |= SENSORUPDATES_pos;
state->vel[0] = this->vel[0];
state->vel[1] = this->vel[1];
state->vel[2] = -this->state[1];
state->vel[2] = -this->velocityState;
state->updated |= SENSORUPDATES_vel;
}
}
@ -203,6 +203,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
return 0;
}
void settingsUpdatedCb(UAVObjEvent *ev)
{
if (ev->obj == AltitudeFilterSettingsHandle()) {
reloadSettings = true;
}
}
/**
* @}

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

@ -36,38 +36,68 @@
// Private constants
#define STACK_REQUIRED 64
#define STACK_REQUIRED 128
#define INIT_CYCLES 100
// Private types
struct data {
float baroOffset;
float baroGPSOffsetCorrectionAlpha;
float baroAlt;
float gpsAlt;
int16_t first_run;
bool useGPS;
};
// Private variables
// Private functions
static int32_t init(stateFilter *self);
static int32_t initwithgps(stateFilter *self);
static int32_t initwithoutgps(stateFilter *self);
static int32_t maininit(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
int32_t filterBaroInitialize(stateFilter *handle)
{
handle->init = &init;
handle->init = &initwithgps;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init(stateFilter *self)
int32_t filterBaroiInitialize(stateFilter *handle)
{
handle->init = &initwithoutgps;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t initwithgps(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->useGPS = 1;
return maininit(self);
}
static int32_t initwithoutgps(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->useGPS = 0;
return maininit(self);
}
static int32_t maininit(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->baroOffset = 0.0f;
this->first_run = 100;
this->gpsAlt = 0.0f;
this->first_run = INIT_CYCLES;
RevoSettingsInitialize();
RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
@ -80,17 +110,29 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
struct data *this = (struct data *)self->localdata;
if (this->first_run) {
// Make sure initial location is initialized properly before continuing
if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
if (this->first_run == INIT_CYCLES) {
this->gpsAlt = state->pos[2];
this->first_run--;
}
}
// Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->baroOffset = (100.f - this->first_run) / 100.f * this->baroOffset + (this->first_run / 100.f) * state->baro[0];
this->baroAlt = this->baroOffset;
this->first_run--;
if (this->first_run < INIT_CYCLES || !this->useGPS) {
this->baroOffset = (((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)) * this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * (state->baro[0] + this->gpsAlt);
this->baroAlt = state->baro[0];
this->first_run--;
}
UNSET_MASK(state->updated, SENSORUPDATES_baro);
}
// make sure we raise an error until properly initialized - would not be good if people arm and
// use altitudehold without initialized barometer filter
return 2;
} else {
// Track barometric altitude offset with a low pass filter
// based on GPS altitude if available
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
(1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
}
@ -99,9 +141,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->baroAlt = state->baro[0];
state->baro[0] -= this->baroOffset;
}
return 0;
}
return 0;
}

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

@ -46,6 +46,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) \
@ -67,10 +69,9 @@ struct data {
stateEstimation work;
uint32_t ins_last_time;
bool inited;
bool inited;
float dTa;
PiOSDeltatimeConfig dtconfig;
};
// Private variables
@ -155,11 +156,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;
@ -225,17 +225,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
@ -301,7 +291,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
@ -336,7 +326,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
@ -356,7 +346,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)) {
SystemAlarmsAlarmData alarms;

2
flight/modules/StateEstimation/filtermag.c
View File

@ -152,7 +152,7 @@ static void checkMagValidity(struct data *this, float mag[3])
}
} else {
if (this->errorcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
} else {
this->errorcount++;
}

1
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -64,6 +64,7 @@ typedef struct stateFilterStruct {
int32_t filterMagInitialize(stateFilter *handle);
int32_t filterBaroiInitialize(stateFilter *handle);
int32_t filterBaroInitialize(stateFilter *handle);
int32_t filterAltitudeInitialize(stateFilter *handle);
int32_t filterAirInitialize(stateFilter *handle);

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

@ -31,6 +31,8 @@
#include "inc/stateestimation.h"
#include <callbackinfo.h>
#include <gyrosensor.h>
#include <accelsensor.h>
#include <magsensor.h>
@ -38,6 +40,7 @@
#include <airspeedsensor.h>
#include <gpspositionsensor.h>
#include <gpsvelocitysensor.h>
#include <homelocation.h>
#include <gyrostate.h>
#include <accelstate.h>
@ -53,10 +56,14 @@
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define TIMEOUT_MS 10
#define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define TIMEOUT_MS 10
// Private filter init const
#define FILTER_INIT_FORCE -1
#define FILTER_INIT_IF_POSSIBLE -2
// local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_LOAD after the update of states updated!
#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(sensorname, shortname, a1, a2, a3) \
@ -72,6 +79,7 @@
UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
} \
}
#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, EXTRACHECK) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
@ -84,6 +92,19 @@
} \
}
#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, a2, EXTRACHECK) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
sensorname##Get(&s); \
if (IS_REAL(s.a1) && IS_REAL(s.a2) && EXTRACHECK) { \
states.shortname[0] = s.a1; \
states.shortname[1] = s.a2; \
} \
else { \
UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
} \
}
// local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_SAVE before the check of alarms!
#define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(statename, shortname, a1, a2, a3) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
@ -94,6 +115,7 @@
s.a3 = states.shortname[2]; \
statename##Set(&s); \
}
#define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(statename, shortname, a1, a2) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
statename##Data s; \
@ -103,8 +125,8 @@
statename##Set(&s); \
}
// Private types
// Private types
struct filterPipelineStruct;
typedef const struct filterPipelineStruct {
@ -117,12 +139,13 @@ static DelayedCallbackInfo *stateEstimationCallback;
static volatile RevoSettingsData revoSettings;
static volatile sensorUpdates updatedSensors;
static int32_t fusionAlgorithm = -1;
static filterPipeline *filterChain = NULL;
static volatile int32_t fusionAlgorithm = -1;
static const filterPipeline *filterChain = NULL;
// different filters available to state estimation
static stateFilter magFilter;
static stateFilter baroFilter;
static stateFilter baroiFilter;
static stateFilter altitudeFilter;
static stateFilter airFilter;
static stateFilter stationaryFilter;
@ -133,20 +156,33 @@ static stateFilter ekf13iFilter;
static stateFilter ekf13Filter;
// preconfigured filter chains selectable via revoSettings.FusionAlgorithm
static filterPipeline *cfQueue = &(filterPipeline) {
static const filterPipeline *cfQueue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &llaFilter,
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfFilter,
.next = NULL,
}
.filter = &cfFilter,
.next = NULL,
}
}
}
}
};
static const filterPipeline *cfmiQueue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfmFilter,
.next = NULL,
}
}
}
@ -176,15 +212,12 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) {
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &llaFilter,
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.filter = &stationaryFilter,
.next = &(filterPipeline) {
.filter = &stationaryFilter,
.next = &(filterPipeline) {
.filter = &ekf13iFilter,
.next = NULL,
}
.filter = &ekf13iFilter,
.next = NULL,
}
}
}
@ -211,6 +244,7 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) {
static void settingsUpdatedCb(UAVObjEvent *objEv);
static void sensorUpdatedCb(UAVObjEvent *objEv);
static void homeLocationUpdatedCb(UAVObjEvent *objEv);
static void StateEstimationCb(void);
static inline int32_t maxint32_t(int32_t a, int32_t b)
@ -236,6 +270,8 @@ int32_t StateEstimationInitialize(void)
GPSVelocitySensorInitialize();
GPSPositionSensorInitialize();
HomeLocationInitialize();
GyroStateInitialize();
AccelStateInitialize();
MagStateInitialize();
@ -245,6 +281,8 @@ int32_t StateEstimationInitialize(void)
RevoSettingsConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&homeLocationUpdatedCb);
GyroSensorConnectCallback(&sensorUpdatedCb);
AccelSensorConnectCallback(&sensorUpdatedCb);
MagSensorConnectCallback(&sensorUpdatedCb);
@ -256,6 +294,7 @@ int32_t StateEstimationInitialize(void)
uint32_t stack_required = STACK_SIZE_BYTES;
// Initialize Filters
stack_required = maxint32_t(stack_required, filterMagInitialize(&magFilter));
stack_required = maxint32_t(stack_required, filterBaroiInitialize(&baroiFilter));
stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter));
stack_required = maxint32_t(stack_required, filterAltitudeInitialize(&altitudeFilter));
stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter));
@ -266,7 +305,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;
}
@ -297,7 +336,7 @@ static void StateEstimationCb(void)
static int8_t alarm = 0;
static int8_t lastAlarm = -1;
static uint16_t alarmcounter = 0;
static filterPipeline *current;
static const filterPipeline *current;
static stateEstimation states;
static uint32_t last_time;
static uint16_t bootDelay = 64;
@ -305,7 +344,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;
}
@ -327,26 +366,29 @@ static void StateEstimationCb(void)
if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
FlightStatusData fs;
FlightStatusGet(&fs);
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) {
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
const filterPipeline *newFilterChain;
switch (revoSettings.FusionAlgorithm) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
newFilterChain = cfQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
newFilterChain = cfmiQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
newFilterChain = cfmQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
newFilterChain = ekf13iQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR:
case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
newFilterChain = ekf13Queue;
break;
default:
newFilterChain = NULL;
}
// initialize filters in chain
current = (filterPipeline *)newFilterChain;
current = newFilterChain;
bool error = 0;
while (current != NULL) {
int32_t result = current->filter->init((stateFilter *)current->filter);
@ -361,7 +403,7 @@ static void StateEstimationCb(void)
return;
} else {
// set new fusion algortithm
filterChain = (filterPipeline *)newFilterChain;
filterChain = newFilterChain;
fusionAlgorithm = revoSettings.FusionAlgorithm;
}
}
@ -377,18 +419,18 @@ static void StateEstimationCb(void)
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
// GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
// at this point sensor state is stored in "states" with some rudimentary filtering applied
// apply all filters in the current filter chain
current = (filterPipeline *)filterChain;
current = filterChain;
// we are not done, re-dispatch self execution
runState = RUNSTATE_FILTER;
DelayedCallbackDispatch(stateEstimationCallback);
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
break;
case RUNSTATE_FILTER:
@ -405,7 +447,7 @@ static void StateEstimationCb(void)
if (!current) {
runState = RUNSTATE_SAVE;
}
DelayedCallbackDispatch(stateEstimationCallback);
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
break;
case RUNSTATE_SAVE:
@ -458,9 +500,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;
}
@ -475,6 +517,17 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
RevoSettingsGet((RevoSettingsData *)&revoSettings);
}
/**
* Callback for eventdispatcher when HomeLocation has been updated
*/
static void homeLocationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// Ask for a filter init (necessary for LLA filter)
// Only possible if disarmed
fusionAlgorithm = FILTER_INIT_IF_POSSIBLE;
}
/**
* Callback for eventdispatcher when any sensor UAVObject has been updated
* updates the list of "recently updated UAVObjects" and dispatches the state estimator callback
@ -513,7 +566,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
updatedSensors |= SENSORUPDATES_airspeed;
}
DelayedCallbackDispatch(stateEstimationCallback);
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
}

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

@ -52,7 +52,7 @@
#include <i2cstats.h>
#include <taskinfo.h>
#include <watchdogstatus.h>
#include <taskinfo.h>
#include <callbackinfo.h>
#include <hwsettings.h>
#include <pios_flashfs.h>
#if defined(PIOS_INCLUDE_RFM22B)
@ -92,11 +92,9 @@
// Private types
// Private variables
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 +103,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,13 +123,14 @@ 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);
// Register task
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
return 0;
}
@ -147,6 +147,7 @@ int32_t SystemModInitialize(void)
ObjectPersistenceInitialize();
#ifdef DIAG_TASKS
TaskInfoInitialize();
CallbackInfoInitialize();
#endif
#ifdef DIAG_I2C_WDG_STATS
I2CStatsInitialize();
@ -175,7 +176,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,
@ -184,15 +185,12 @@ static void systemTask(__attribute__((unused)) void *parameters)
*/
PIOS_SYS_Reset();
}
#if defined(PIOS_INCLUDE_IAP)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
// Listen for SettingPersistance object updates, connect a callback function
ObjectPersistenceConnectQueue(objectPersistenceQueue);
@ -204,14 +202,16 @@ static void systemTask(__attribute__((unused)) void *parameters)
#ifdef DIAG_TASKS
TaskInfoData taskInfoData;
CallbackInfoData callbackInfoData;
#endif
// Main system loop
while (1) {
// Update the system statistics
updateStats();
cycleCount = cycleCount > 0 ? cycleCount - 1 : 7;
cycleCount = cycleCount > 0 ? cycleCount - 1 : 7;
// if(cycleCount == 1){
updateStats();
// Update the system alarms
updateSystemAlarms();
#ifdef DIAG_I2C_WDG_STATS
@ -223,8 +223,13 @@ 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
// if(FALSE){
PIOS_CALLBACKSCHEDULER_ForEachCallback(callbackSchedulerForEachCallback, &callbackInfoData);
CallbackInfoSet(&callbackInfoData);
// }
#endif
// }
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
uint8_t armingStatus;
@ -440,7 +445,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
@ -516,26 +540,23 @@ static uint16_t GetFreeIrqStackSize(void)
*/
static void updateStats()
{
static portTickType lastTickCount = 0;
SystemStatsData stats;
// Get stats and update
SystemStatsGet(&stats);
stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
// POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
stats.SystemModStackRemaining = 128;
stats.HeapRemaining = 10240;
#else
stats.HeapRemaining = xPortGetFreeHeapSize();
stats.SystemModStackRemaining = uxTaskGetStackHighWaterMark(NULL) * 4;
#endif
// Get Irq stack status
stats.IRQStackRemaining = GetFreeIrqStackSize();
// When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
if (idleCounterClear) {
idleCounter = 0;
}
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
if (pios_uavo_settings_fs_id) {
PIOS_FLASHFS_GetStats(pios_uavo_settings_fs_id, &fsStats);
@ -548,16 +569,11 @@ static void updateStats()
stats.UsrSlotsActive = fsStats.num_active_slots;
}
#endif
portTickType now = xTaskGetTickCount();
if (now > lastTickCount) {
uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS; // in ms
stats.CPULoad = 100 - (uint8_t)roundf(100.0f * ((float)idleCounter / ((float)dT / 1000.0f)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
} // else: TickCount has wrapped, do not calc now
lastTickCount = now;
idleCounterClear = 1;
stats.CPULoad = 100 - PIOS_TASK_MONITOR_GetIdlePercentage();
#if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
float temp_voltage = PIOS_ADC_PinGetVolt(PIOS_ADC_TEMPERATURE_PIN);
stats.CPUTemp = PIOS_CONVERT_VOLT_TO_CPU_TEMP(temp_voltage);;
stats.CPUTemp = PIOS_CONVERT_VOLT_TO_CPU_TEMP(temp_voltage);;
#endif
SystemStatsSet(&stats);
}
@ -600,10 +616,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
@ -631,15 +652,7 @@ static void updateSystemAlarms()
* Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
*/
void vApplicationIdleHook(void)
{
// Called when the scheduler has no tasks to run
if (idleCounterClear == 0) {
++idleCounter;
} else {
idleCounter = 0;
idleCounterClear = 0;
}
}
{}
/**
* Called by the RTOS when a stack overflow is detected.
@ -648,7 +661,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) {

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

@ -40,27 +40,41 @@
#include "taskinfo.h"
// Private constants
#define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE
#define STACK_SIZE_BYTES PIOS_TELEM_STACK_SIZE
#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
#define CONNECTION_TIMEOUT_MS 8000
#define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE
// Three different stack size parameter are accepted for Telemetry(RX PIOS_TELEM_RX_STACK_SIZE)
// Tx(PIOS_TELEM_TX_STACK_SIZE) and Radio RX(PIOS_TELEM_RADIO_RX_STACK_SIZE)
#ifdef PIOS_TELEM_RX_STACK_SIZE
#define STACK_SIZE_RX_BYTES PIOS_TELEM_RX_STACK_SIZE
#define STACK_SIZE_TX_BYTES PIOS_TELEM_TX_STACK_SIZE
#else
#define STACK_SIZE_RX_BYTES PIOS_TELEM_STACK_SIZE
#define STACK_SIZE_TX_BYTES PIOS_TELEM_STACK_SIZE
#endif
#ifdef PIOS_TELEM_RADIO_RX_STACK_SIZE
#define STACK_SIZE_RADIO_RX_BYTES PIOS_TELEM_RADIO_RX_STACK_SIZE
#else
#define STACK_SIZE_RADIO_RX_BYTES STACK_SIZE_RX_BYTES
#endif
#define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_RADRX (tskIDLE_PRIORITY + 2)
#define REQ_TIMEOUT_MS 250
#define MAX_RETRIES 2
#define STATS_UPDATE_PERIOD_MS 4000
#define CONNECTION_TIMEOUT_MS 8000
// Private types
// Private variables
static uint32_t telemetryPort;
#ifdef PIOS_INCLUDE_RFM22B
static uint32_t radioPort;
#endif
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
@ -83,6 +97,7 @@ static void telemetryTxTask(void *parameters);
static void telemetryRxTask(void *parameters);
#ifdef PIOS_INCLUDE_RFM22B
static void radioRxTask(void *parameters);
static int32_t transmitRadioData(uint8_t *data, int32_t length);
#endif
static int32_t transmitData(uint8_t *data, int32_t length);
static void registerObject(UAVObjHandle obj);
@ -109,21 +124,16 @@ int32_t TelemetryStart(void)
GCSTelemetryStatsConnectQueue(priorityQueue);
// Start telemetry tasks
xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_TX_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle);
xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_RX_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
#ifdef PIOS_INCLUDE_RFM22B
xTaskCreate(radioRxTask, (signed char *)"RadioRx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle);
xTaskCreate(radioRxTask, (signed char *)"RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle);
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;
}
@ -148,16 +158,20 @@ int32_t TelemetryInitialize(void)
// Update telemetry settings
telemetryPort = PIOS_COM_TELEM_RF;
#ifdef PIOS_INCLUDE_RFM22B
radioPort = PIOS_COM_RF;
#endif
HwSettingsInitialize();
updateSettings();
// Initialise UAVTalk
uavTalkCon = UAVTalkInitialize(&transmitData);
#ifdef PIOS_INCLUDE_RFM22B
radioUavTalkCon = UAVTalkInitialize(&transmitData);
radioUavTalkCon = UAVTalkInitialize(&transmitRadioData);
#endif
// Create periodic event that will be used to update the telemetry stats
// FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms...
txErrors = 0;
txRetries = 0;
UAVObjEvent ev;
@ -177,22 +191,10 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart);
static void registerObject(UAVObjHandle obj)
{
if (UAVObjIsMetaobject(obj)) {
/* Only connect change notifications for meta objects. No periodic updates */
// Only connect change notifications for meta objects. No periodic updates
UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES);
return;
} else {
// Setup object for periodic updates
UAVObjEvent ev = {
.obj = obj,
.instId = UAVOBJ_ALL_INSTANCES,
.event = EV_UPDATED_PERIODIC,
};
EventPeriodicQueueCreate(&ev, queue, 0);
ev.event = EV_LOGGING_PERIODIC;
EventPeriodicQueueCreate(&ev, queue, 0);
// Setup object for telemetry updates
updateObject(obj, EV_NONE);
}
}
@ -208,9 +210,8 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
int32_t eventMask;
if (UAVObjIsMetaobject(obj)) {
/* This function updates the periodic updates for the object.
* Meta Objects cannot have periodic updates.
*/
// This function updates the periodic updates for the object.
// Meta Objects cannot have periodic updates.
PIOS_Assert(false);
return;
}
@ -288,7 +289,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);
}
}
/**
@ -298,7 +304,6 @@ static void processObjEvent(UAVObjEvent *ev)
{
UAVObjMetadata metadata;
UAVObjUpdateMode updateMode;
FlightTelemetryStatsData flightStats;
int32_t retries;
int32_t success;
@ -307,7 +312,6 @@ static void processObjEvent(UAVObjEvent *ev)
} else if (ev->obj == GCSTelemetryStatsHandle()) {
gcsTelemetryStatsUpdated();
} else {
FlightTelemetryStatsGet(&flightStats);
// Get object metadata
UAVObjGetMetadata(ev->obj, &metadata);
updateMode = UAVObjGetTelemetryUpdateMode(&metadata);
@ -315,32 +319,41 @@ static void processObjEvent(UAVObjEvent *ev)
// Act on event
retries = 0;
success = -1;
if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED))
|| ev->event == EV_UPDATED_MANUAL
|| (ev->event == EV_UPDATED_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) {
// Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
++retries;
// call blocks until ack is received or timeout
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);
if (success == -1) {
++retries;
}
}
// Update stats
txRetries += (retries - 1);
txRetries += retries;
if (success == -1) {
++txErrors;
}
} else if (ev->event == EV_UPDATE_REQ) {
// Request object update from GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
++retries;
// call blocks until update is received or timeout
success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS);
if (success == -1) {
++retries;
}
}
// Update stats
txRetries += (retries - 1);
txRetries += retries;
if (success == -1) {
++txErrors;
}
}
// If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) {
updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for
// linked object will be the actual object the metadata are for
updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE);
} else {
if (updateMode == UPDATEMODE_THROTTLED) {
// If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
@ -351,7 +364,9 @@ static void processObjEvent(UAVObjEvent *ev)
// Log UAVObject if necessary
if (ev->obj) {
updateMode = UAVObjGetLoggingUpdateMode(&metadata);
if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_LOGGING_MANUAL || ((ev->event == EV_LOGGING_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED))
|| ev->event == EV_LOGGING_MANUAL
|| (ev->event == EV_LOGGING_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) {
if (ev->instId == UAVOBJ_ALL_INSTANCES) {
success = UAVObjGetNumInstances(ev->obj);
for (retries = 0; retries < success; retries++) {
@ -377,32 +392,34 @@ 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) {
// Wait for queue message
if (xQueueReceive(priorityQueue, &ev, portMAX_DELAY) == pdTRUE) {
// empty priority queue, non-blocking
while (xQueueReceive(priorityQueue, &ev, 0) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
// check regular queue and process update - non-blocking
if (xQueueReceive(queue, &ev, 0) == pdTRUE) {
// Process event
processObjEvent(&ev);
// if both queues are empty, wait on priority queue for updates (1 tick) then repeat cycle
} else if (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
#else
// wait on queue for updates (1 tick) then repeat cycle
if (xQueueReceive(queue, &ev, 1) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
#endif /* if defined(PIOS_TELEM_PRIORITY_QUEUE) */
}
}
#endif
/**
* Telemetry receive task. Processes queue events and periodic updates.
@ -438,12 +455,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
{
// Task loop
while (1) {
if (telemetryPort) {
if (radioPort) {
// Block until data are available
uint8_t serial_data[1];
uint16_t bytes_to_process;
bytes_to_process = PIOS_COM_ReceiveBuffer(telemetryPort, serial_data, sizeof(serial_data), 500);
bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]);
@ -454,6 +471,21 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
}
}
}
/**
* Transmit data buffer to the radioport.
* \param[in] data Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitRadioData(uint8_t *data, int32_t length)
{
if (radioPort) {
return PIOS_COM_SendBuffer(radioPort, data, length);
}
return -1;
}
#endif /* PIOS_INCLUDE_RFM22B */
/**
@ -484,12 +516,21 @@ static int32_t transmitData(uint8_t *data, int32_t length)
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
{
UAVObjEvent ev;
int32_t ret;
// Add object for periodic updates
// Add or update object for periodic updates
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_UPDATED_PERIODIC;
return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
ev.lowPriority = true;
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
if (ret == -1) {
ret = EventPeriodicQueueCreate(&ev, targetQueue, updatePeriodMs);
}
return ret;
}
/**
@ -502,12 +543,21 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
{
UAVObjEvent ev;
int32_t ret;
// Add object for periodic updates
// Add or update object for periodic updates
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_LOGGING_PERIODIC;
return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
ev.lowPriority = true;
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
if (ret == -1) {
ret = EventPeriodicQueueCreate(&ev, targetQueue, updatePeriodMs);
}
return ret;
}
/**
@ -540,12 +590,10 @@ static void updateTelemetryStats()
uint32_t timeNow;
// Get stats
UAVTalkGetStats(uavTalkCon, &utalkStats);
UAVTalkGetStats(uavTalkCon, &utalkStats, true);
#ifdef PIOS_INCLUDE_RFM22B
UAVTalkAddStats(radioUavTalkCon, &utalkStats);
UAVTalkResetStats(radioUavTalkCon);
UAVTalkAddStats(radioUavTalkCon, &utalkStats, true);
#endif
UAVTalkResetStats(uavTalkCon);
// Get object data
FlightTelemetryStatsGet(&flightStats);
@ -553,25 +601,33 @@ static void updateTelemetryStats()
// Update stats object
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.RxFailures += utalkStats.rxErrors;
flightStats.TxFailures += txErrors;
flightStats.TxRetries += txRetries;
txErrors = 0;
txRetries = 0;
flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.TxBytes += utalkStats.txBytes;
flightStats.TxFailures += txErrors;
flightStats.TxRetries += txRetries;
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.RxBytes += utalkStats.rxBytes;
flightStats.RxFailures += utalkStats.rxErrors;
flightStats.RxSyncErrors += utalkStats.rxSyncErrors;
flightStats.RxCrcErrors += utalkStats.rxCrcErrors;
} else {
flightStats.RxDataRate = 0;
flightStats.TxDataRate = 0;
flightStats.RxFailures = 0;
flightStats.TxFailures = 0;
flightStats.TxRetries = 0;
txErrors = 0;
txRetries = 0;
flightStats.TxDataRate = 0;
flightStats.TxBytes = 0;
flightStats.TxFailures = 0;
flightStats.TxRetries = 0;
flightStats.RxDataRate = 0;
flightStats.RxBytes = 0;
flightStats.RxFailures = 0;
flightStats.RxSyncErrors = 0;
flightStats.RxCrcErrors = 0;
}
txErrors = 0;
txRetries = 0;
// Check for connection timeout
timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
if (utalkStats.rxObjects > 0) {
timeOfLastObjectUpdate = timeNow;
}
@ -666,17 +722,25 @@ static void updateSettings()
* Determine input/output com port as highest priority available
* @param[in] input Returns the approproate input com port if true, else the appropriate output com port
*/
#ifdef PIOS_INCLUDE_RFM22B
static uint32_t getComPort(bool input)
{
#ifndef PIOS_INCLUDE_RFM22B
input = false;
#else
static uint32_t getComPort(__attribute__((unused)) bool input)
#endif
{
#if defined(PIOS_INCLUDE_USB)
// if USB is connected, USB takes precedence for telemetry
if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) {
return PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB */
if (!input && PIOS_COM_Available(telemetryPort)) {
#ifdef PIOS_INCLUDE_RFM22B
// PIOS_COM_RF input is handled by a separate RX thread and therefore must be ignored
if (input && telemetryPort == PIOS_COM_RF) {
return 0;
} else
#endif /* PIOS_INCLUDE_RFM22B */
if (PIOS_COM_Available(telemetryPort)) {
return telemetryPort;
} else {
return 0;

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

@ -56,6 +56,10 @@
#include "accessorydesired.h"
#include "manualcontrolcommand.h"
#include "stabilizationsettings.h"
#include "stabilizationbank.h"
#include "stabilizationsettingsbank1.h"
#include "stabilizationsettingsbank2.h"
#include "stabilizationsettingsbank3.h"
#include "flightstatus.h"
#include "hwsettings.h"
@ -107,6 +111,7 @@ int32_t TxPIDInitialize(void)
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
@ -163,11 +168,29 @@ static void updatePIDs(UAVObjEvent *ev)
return;
}
StabilizationBankData bank;
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
default:
return;
}
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
AccessoryDesiredData accessory;
uint8_t needsUpdate = 0;
uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0;
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
@ -192,107 +215,125 @@ static void updatePIDs(UAVObjEvent *ev)
switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdate |= update(&stab.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdate |= update(&stab.RollRatePID.Ki, value);
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdate |= update(&stab.RollRatePID.Kd, value);
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdate |= update(&stab.RollPI.Kp, value);
needsUpdateBank |= update(&bank.RollPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdate |= update(&stab.RollPI.Ki, value);
needsUpdateBank |= update(&bank.RollPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdate |= update(&stab.PitchRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdate |= update(&stab.PitchRatePID.Ki, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdate |= update(&stab.PitchRatePID.Kd, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdate |= update(&stab.PitchRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdate |= update(&stab.PitchPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdate |= update(&stab.PitchPI.Ki, value);
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.PitchPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdate |= update(&stab.RollRatePID.Kp, value);
needsUpdate |= update(&stab.PitchRatePID.Kp, value);
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdate |= update(&stab.RollRatePID.Ki, value);
needsUpdate |= update(&stab.PitchRatePID.Ki, value);
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdate |= update(&stab.RollRatePID.Kd, value);
needsUpdate |= update(&stab.PitchRatePID.Kd, value);
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID.ILimit, value);
needsUpdate |= update(&stab.PitchRatePID.ILimit, value);
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdate |= update(&stab.RollPI.Kp, value);
needsUpdate |= update(&stab.PitchPI.Kp, value);
needsUpdateBank |= update(&bank.RollPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdate |= update(&stab.RollPI.Ki, value);
needsUpdate |= update(&stab.PitchPI.Ki, value);
needsUpdateBank |= update(&bank.RollPI.Ki, value);
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI.ILimit, value);
needsUpdate |= update(&stab.PitchPI.ILimit, value);
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdate |= update(&stab.YawRatePID.Kp, value);
needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdate |= update(&stab.YawRatePID.Ki, value);
needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdate |= update(&stab.YawRatePID.Kd, value);
needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdate |= update(&stab.YawRatePID.ILimit, value);
needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdate |= update(&stab.YawPI.Kp, value);
needsUpdateBank |= update(&bank.YawPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdate |= update(&stab.YawPI.Ki, value);
needsUpdateBank |= update(&bank.YawPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdate |= update(&stab.YawPI.ILimit, value);
needsUpdateBank |= update(&bank.YawPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdate |= update(&stab.GyroTau, value);
needsUpdateStab |= update(&stab.GyroTau, value);
break;
default:
PIOS_Assert(0);
}
}
}
if (needsUpdate) {
if (needsUpdateStab) {
StabilizationSettingsSet(&stab);
}
if (needsUpdateBank) {
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
break;
default:
return;
}
}
}
/**

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

@ -55,7 +55,7 @@
#include "hwsettings.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionstate.h"
#include "manualcontrol.h"
#include "manualcontrolcommand.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "gpsvelocitysensor.h"
@ -65,6 +65,7 @@
#include "nedaccel.h"
#include "stabilizationdesired.h"
#include "stabilizationsettings.h"
#include "stabilizationbank.h"
#include "systemsettings.h"
#include "velocitydesired.h"
#include "velocitystate.h"
@ -157,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.
*/
@ -205,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;
@ -262,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);
@ -473,48 +470,14 @@ void updateEndpointVelocity()
float eastCommand;
float downCommand;
float northPos = 0, eastPos = 0, downPos = 0;
switch (vtolpathfollowerSettings.PositionSource) {
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
northPos = positionState.North;
eastPos = positionState.East;
downPos = positionState.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
{
// this used to work with the NEDposition UAVObject
// however this UAVObject has been removed
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
float alt = homeLocation.Altitude;
float T[3] = { alt + 6.378137E6f,
cosf(lat) * (alt + 6.378137E6f),
-1.0f };
float NED[3] = { T[0] * (DEG2RAD((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f)),
T[1] * (DEG2RAD((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f)),
T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) };
northPos = NED[0];
eastPos = NED[1];
downPos = NED[2];
}
break;
default:
PIOS_Assert(0);
break;
}
// Compute desired north command
northError = pathDesired.End.North - northPos;
northError = pathDesired.End.North - positionState.North;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral);
eastError = pathDesired.End.East - eastPos;
eastError = pathDesired.End.East - positionState.East;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
@ -530,7 +493,7 @@ void updateEndpointVelocity()
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
downError = pathDesired.End.Down - downPos;
downError = pathDesired.End.Down - positionState.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
@ -549,10 +512,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;
@ -575,7 +538,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
NedAccelData nedAccel;
StabilizationSettingsData stabSettings;
StabilizationBankData stabSettings;
SystemSettingsData systemSettings;
float northError;
@ -593,17 +556,17 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
StabilizationSettingsGet(&stabSettings);
StabilizationBankGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = 0, eastVel = 0, downVel = 0;
switch (vtolpathfollowerSettings.VelocitySource) {
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_STATE_ESTIMATION:
northVel = velocityState.North;
eastVel = velocityState.East;
downVel = velocityState.Down;
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_VELNED:
{
GPSVelocitySensorData gpsVelocity;
GPSVelocitySensorGet(&gpsVelocity);
@ -612,7 +575,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
downVel = gpsVelocity.Down;
}
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_GROUNDSPEED:
{
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
@ -652,13 +615,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
@ -669,11 +632,11 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) {
// For now override thrust with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Throttle = manualControl.Throttle;
stabDesired.Thrust = manualControl.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;

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

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

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

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

22
flight/pios/common/pios_crc.c
View File

@ -64,6 +64,17 @@ static const uint16_t CRC_Table16[] = { // HDLC polynomial
0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
};
/**
* Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
* using the configuration:
* Width = 8
* Poly = 0x07
* XorIn = 0x00
* ReflectIn = False
* XorOut = 0x00
* ReflectOut = False
* Algorithm = table-driven
*/
static const uint32_t CRC_Table32[] = {
0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005,
0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd,
@ -101,17 +112,6 @@ static const uint32_t CRC_Table32[] = {
/**
* Update the crc value with new data.
*
* Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
* using the configuration:
* Width = 8
* Poly = 0x07
* XorIn = 0x00
* ReflectIn = False
* XorOut = 0x00
* ReflectOut = False
* Algorithm = table-driven
*
* \param crc The current crc value.
* \param data Pointer to a buffer of \a data_len bytes.
* \param length Number of bytes in the \a data buffer.

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

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

84
flight/pios/common/pios_ms4525do.c Normal file
View File

@ -0,0 +1,84 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_MS4525DO MS4525DO Functions
* @brief Hardware functions to deal with the PixHawk Airspeed Sensor based on MS4525DO
* @{
*
* @file pios_ms4525do.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief PixHawk MS4525DO Airspeed Sensor Driver
* @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"
#ifdef PIOS_INCLUDE_MS4525DO
/* Local Defs and Variables */
static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len)
{
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = MS4525DO_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(PIOS_I2C_MS4525DO_ADAPTER, txn_list, NELEMENTS(txn_list));
}
// values has to ba an arrray with two elements
// values stay untouched on error
int8_t PIOS_MS4525DO_Read(uint16_t *values)
{
uint8_t data[4];
int8_t retVal = PIOS_MS4525DO_ReadI2C(data, sizeof(data));
uint8_t status = data[0] & 0xC0;
if (status == 0x80) {
/* stale data */
return -5;
} else if (status == 0xC0) {
/* device probably broken */
return -6;
}
/* differential pressure */
values[0] = (data[0] << 8);
values[0] += data[1];
/* temperature */
values[1] = (data[2] << 8);
values[1] += data[3];
values[1] = (values[1] >> 5);
return retVal;
}
#endif /* PIOS_INCLUDE_MS4525DO */

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

@ -32,6 +32,7 @@
#ifdef PIOS_INCLUDE_MS5611
#define POW2(x) (1 << x)
// TODO: Clean this up. Getting around old constant.
#define PIOS_MS5611_OVERSAMPLING oversampling
@ -52,6 +53,9 @@ static int32_t lastConversionStart;
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
// Second order temperature compensation. Temperature offset
static int64_t compensation_t2;
// Move into proper driver structure with cfg stored
static uint32_t oversampling;
static const struct pios_ms5611_cfg *dev_cfg;
@ -74,6 +78,9 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
uint8_t data[2];
// reset temperature compensation values
compensation_t2 = 0;
/* Calibration parameters */
for (int i = 0; i < 6; i++) {
PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
@ -160,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)
@ -184,24 +190,57 @@ 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;
// used for second order temperature compensation
int64_t Offset2 = 0;
int64_t Sens2 = 0;
/* Read the pressure conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1;
}
// check if temperature is less than 20°C
if (Temperature < 2000) {
// 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) {
// 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;
Offset2 = 0;
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);
Sens = ((int64_t)CalibData.C[0]) << 15;
Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8);
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;
}
@ -211,7 +250,8 @@ int32_t PIOS_MS5611_ReadADC(void)
*/
float PIOS_MS5611_GetTemperature(void)
{
return ((float)Temperature) / 100.0f;
// Apply the second order low and very low temperature compensation offset
return ((float)(Temperature - compensation_t2)) / 100.0f;
}
/**

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

@ -64,7 +64,7 @@
/* Local Defines */
#define STACK_SIZE_BYTES 200
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // flight control relevant device driver (ppm link)
#define ISR_TIMEOUT 1 // ms
#define EVENT_QUEUE_SIZE 5
#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600
@ -139,7 +139,11 @@ static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = {
PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,
PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE
}; // 64 bytes
static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1) / 2 + 2] = { PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2 };
static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1) / 2 + 2] = {
PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2
};
static const uint8_t OUT_FF[64] = {
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
@ -150,8 +154,21 @@ static const uint8_t OUT_FF[64] = {
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
};
// The randomized channel list.
static const uint8_t channel_list[] = { 68, 34, 2, 184, 166, 94, 204, 18, 47, 118, 239, 176, 5, 213, 218, 186, 104, 160, 199, 209, 231, 197, 92, 191, 88, 129, 40, 19, 93, 200, 156, 14, 247, 182, 193, 194, 208, 210, 248, 76, 244, 48, 179, 105, 25, 74, 155, 203, 39, 97, 195, 81, 83, 180, 134, 172, 235, 132, 198, 119, 207, 154, 0, 61, 140, 171, 245, 26, 95, 3, 22, 62, 169, 55, 127, 144, 45, 33, 170, 91, 158, 167, 63, 201, 41, 21, 190, 51, 103, 49, 189, 205, 240, 89, 181, 149, 6, 157, 249, 230, 115, 72, 163, 17, 29, 99, 28, 117, 219, 73, 78, 53, 69, 216, 161, 124, 110, 242, 214, 145, 13, 11, 220, 113, 138, 58, 54, 162, 237, 37, 152, 187, 232, 77, 126, 85, 38, 238, 173, 23, 188, 100, 131, 226, 31, 9, 114, 106, 221, 42, 233, 139, 4, 241, 96, 211, 8, 98, 121, 147, 24, 217, 27, 87, 122, 125, 135, 148, 178, 71, 206, 57, 141, 35, 30, 246, 159, 16, 32, 15, 229, 20, 12, 223, 150, 101, 79, 56, 102, 111, 174, 236, 137, 143, 52, 225, 64, 224, 112, 168, 243, 130, 108, 202, 123, 146, 228, 75, 46, 153, 7, 192, 175, 151, 222, 59, 82, 90, 1, 65, 109, 44, 165, 84, 43, 36, 128, 196, 67, 80, 136, 86, 70, 234, 66, 185, 10, 164, 177, 116, 50, 107, 183, 215, 212, 60, 227, 133, 120, 142 };
static const uint8_t channel_list[] = {
68, 34, 2, 184, 166, 94, 204, 18, 47, 118, 239, 176, 5, 213, 218, 186, 104, 160, 199, 209, 231, 197, 92,
191, 88, 129, 40, 19, 93, 200, 156, 14, 247, 182, 193, 194, 208, 210, 248, 76, 244, 48, 179, 105, 25, 74,
155, 203, 39, 97, 195, 81, 83, 180, 134, 172, 235, 132, 198, 119, 207, 154, 0, 61, 140, 171, 245, 26, 95,
3, 22, 62, 169, 55, 127, 144, 45, 33, 170, 91, 158, 167, 63, 201, 41, 21, 190, 51, 103, 49, 189, 205,
240, 89, 181, 149, 6, 157, 249, 230, 115, 72, 163, 17, 29, 99, 28, 117, 219, 73, 78, 53, 69, 216, 161,
124, 110, 242, 214, 145, 13, 11, 220, 113, 138, 58, 54, 162, 237, 37, 152, 187, 232, 77, 126, 85, 38, 238,
173, 23, 188, 100, 131, 226, 31, 9, 114, 106, 221, 42, 233, 139, 4, 241, 96, 211, 8, 98, 121, 147, 24,
217, 27, 87, 122, 125, 135, 148, 178, 71, 206, 57, 141, 35, 30, 246, 159, 16, 32, 15, 229, 20, 12, 223,
150, 101, 79, 56, 102, 111, 174, 236, 137, 143, 52, 225, 64, 224, 112, 168, 243, 130, 108, 202, 123, 146, 228,
75, 46, 153, 7, 192, 175, 151, 222, 59, 82, 90, 1, 65, 109, 44, 165, 84, 43, 36, 128, 196, 67, 80,
136, 86, 70, 234, 66, 185, 10, 164, 177, 116, 50, 107, 183, 215, 212, 60, 227, 133, 120, 14
};
/* Local function forwared declarations */
static void pios_rfm22_task(void *parameters);
@ -1221,7 +1238,7 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio
// Something went fairly seriously wrong
rfm22b_dev->errors++;
}
portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken1 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
} else {
// Store the event.
if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) {

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

@ -250,7 +250,7 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
state->received_data[state->byte_count - 1] = b;
state->byte_count++;
} else {
if (b == SBUS_EOF_BYTE) {
if (b == SBUS_EOF_BYTE || (b % SBUS_R7008SB_EOF_COUNTER_MASK) == SBUS_R7008SB_EOF_BYTE) {
/* full frame received */
uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3];
if (flags & SBUS_FLAG_FL) {

34
flight/pios/common/pios_task_monitor.c
View File

@ -33,6 +33,7 @@
static xSemaphoreHandle mLock;
static xTaskHandle *mTaskHandles;
static uint32_t mLastMonitorTime;
static uint32_t mLastIdleMonitorTime;
static uint16_t mMaxTasks;
/**
@ -53,9 +54,11 @@ int32_t PIOS_TASK_MONITOR_Initialize(uint16_t max_tasks)
mMaxTasks = max_tasks;
#if (configGENERATE_RUN_TIME_STATS == 1)
mLastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
mLastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
mLastIdleMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
#else
mLastMonitorTime = 0;
mLastMonitorTime = 0;
mLastIdleMonitorTime = 0;
#endif
return 0;
}
@ -146,4 +149,31 @@ void PIOS_TASK_MONITOR_ForEachTask(TaskMonitorTaskInfoCallback callback, void *c
xSemaphoreGiveRecursive(mLock);
}
uint8_t PIOS_TASK_MONITOR_GetIdlePercentage()
{
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
return 50;
#elif (configGENERATE_RUN_TIME_STATS == 1)
xSemaphoreTakeRecursive(mLock, portMAX_DELAY);
uint32_t currentTime = portGET_RUN_TIME_COUNTER_VALUE();
/* avoid divide-by-zero if the interval is too small */
uint32_t deltaTime = ((currentTime - mLastIdleMonitorTime) / 100) ? : 1;
mLastIdleMonitorTime = currentTime;
uint8_t running_time_percentage = 0;
/* Generate idle time percentage stats */
running_time_percentage = uxTaskGetRunTime(xTaskGetIdleTaskHandle()) / deltaTime;
xSemaphoreGiveRecursive(mLock);
return running_time_percentage;
#else
return 0;
#endif
}
#endif // PIOS_INCLUDE_TASK_MONITOR

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

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

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

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

8
flight/pios/inc/pios_i2c_priv.h
View File

@ -92,8 +92,12 @@ struct pios_i2c_adapter {
uint8_t busy;
#endif
bool bus_error;
bool nack;
/* variables for transfer timeouts */
uint32_t transfer_delay_uS; // approx time to transfer one byte, calculated later basen on setting use here time based on 100 kbits/s
uint32_t transfer_timeout_ticks; // take something tha makes sense for small transaction, calculated later based upon transmission desired
bool bus_error;
bool nack;
volatile enum i2c_adapter_state curr_state;
const struct pios_i2c_txn *first_txn;

45
flight/pios/inc/pios_ms4525do.h Normal file
View File

@ -0,0 +1,45 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_MS4525DO MS4525DO Functions
* @brief Hardware functions to deal with the PixHawk Airspeed Sensor based on MS4525DO
* @{
*
* @file pios_ms4525do.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief PixHawk MS4525DO Airspeed Sensor Driver
* @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_MS4525DO_H
#define PIOS_MS4525DO_H
// Interface Type I chip
#define MS4525DO_I2C_ADDR 0x28
// Interface Type J chip
/* #define MS4525DO_I2C_ADDR 0x36 */
// Interface Type K chip
/* #define MS4525DO_I2C_ADDR 0x46 */
extern int8_t PIOS_MS4525DO_Request(void);
extern int8_t PIOS_MS4525DO_Read(uint16_t *values);
#endif /* PIOS_MS4525DO_H */

24
flight/pios/inc/pios_sbus_priv.h
View File

@ -50,14 +50,24 @@
* 0x08 - failsafe flag,
* 0xf0 - reserved
* 1 byte - 0x00 (end of frame byte)
*
* The R7008SB receiver has four different end of frame bytes, which rotates in order:
* 00000100
* 00010100
* 00100100
* 00110100
*/
#define SBUS_FRAME_LENGTH (1 + 22 + 1 + 1)
#define SBUS_SOF_BYTE 0x0f
#define SBUS_EOF_BYTE 0x00
#define SBUS_FLAG_DC1 0x01
#define SBUS_FLAG_DC2 0x02
#define SBUS_FLAG_FL 0x04
#define SBUS_FLAG_FS 0x08
#define SBUS_FRAME_LENGTH (1 + 22 + 1 + 1)
#define SBUS_SOF_BYTE 0x0f
#define SBUS_EOF_BYTE 0x00
#define SBUS_FLAG_DC1 0x01
#define SBUS_FLAG_DC2 0x02
#define SBUS_FLAG_FL 0x04
#define SBUS_FLAG_FS 0x08
#define SBUS_R7008SB_EOF_COUNTER_MASK 0xCF
#define SBUS_R7008SB_EOF_BYTE 0x04
/*
* S.Bus protocol provides 16 proportional and 2 discrete channels.

5
flight/pios/inc/pios_task_monitor.h
View File

@ -104,4 +104,9 @@ typedef void (*TaskMonitorTaskInfoCallback)(uint16_t task_id, const struct pios_
*/
extern void PIOS_TASK_MONITOR_ForEachTask(TaskMonitorTaskInfoCallback callback, void *context);
/**
* Return the idle task running time percentage.
*/
extern uint8_t PIOS_TASK_MONITOR_GetIdlePercentage();
#endif // PIOS_TASK_MONITOR_H

15
flight/pios/pios.h
View File

@ -93,6 +93,14 @@
#include <pios_task_monitor.h>
#endif
/* PIOS CallbackScheduler */
#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
#ifndef PIOS_INCLUDE_FREERTOS
#error PiOS CallbackScheduler requires PIOS_INCLUDE_FREERTOS to be defined
#endif
#include <pios_callbackscheduler.h>
#endif
/* PIOS bootloader helper */
#ifdef PIOS_INCLUDE_BL_HELPER
/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */
@ -102,6 +110,7 @@
/* PIOS system functions */
#ifdef PIOS_INCLUDE_DELAY
#include <pios_delay.h>
#include <pios_deltatime.h>
#endif
#ifdef PIOS_INCLUDE_INITCALL
@ -220,6 +229,12 @@
#include <pios_etasv3.h>
#endif
#ifdef PIOS_INCLUDE_MS4525DO
/* PixHawk Airspeed Sensor based on MS4525DO */
#include <pios_ms4525do.h>
#endif
#ifdef PIOS_INCLUDE_HCSR04
/* HC-SR04 Ultrasonic Sensor */
#include <pios_hcsr04.h>

9
flight/pios/pios_sim_posix.h
View File

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

0
flight/pios/inc/pios_usb_hid_istr.h → flight/pios/stm32f10x/inc/pios_usb_hid_istr.h
View File

0
flight/pios/inc/pios_usb_hid_pwr.h → flight/pios/stm32f10x/inc/pios_usb_hid_pwr.h
View File

0
flight/pios/inc/pios_usb_rctx.h → flight/pios/stm32f10x/inc/pios_usb_rctx.h
View File

0
flight/pios/inc/pios_usb_rctx_priv.h → flight/pios/stm32f10x/inc/pios_usb_rctx_priv.h
View File

11
flight/pios/inc/usb_conf.h → flight/pios/stm32f10x/inc/usb_conf.h
View File

@ -29,25 +29,24 @@
/* -------------- Buffer Description Table -----------------*/
/*-------------------------------------------------------------*/
/* buffer table base address */
/* buffer table base address */
#define BTABLE_ADDRESS (0x00)
/* EP0 */
/* EP0 (Control) */
/* rx/tx buffer base address */
#define ENDP0_RXADDR (0x20)
#define ENDP0_TXADDR (0x40)
/* EP1 */
/* EP1 (HID) */
/* rx/tx buffer base address */
#define ENDP1_TXADDR (0x60)
#define ENDP1_RXADDR (0x80)
#define ENDP1_RXADDR (0xA0)
/* EP2 */
/* EP2 (CDC Call Control) */
/* rx/tx buffer base address */
#define ENDP2_TXADDR (0x100)
#define ENDP2_RXADDR (0x140)
/* EP3 */
/* EP3 (CDC Data) */
/* rx/tx buffer base address */
#define ENDP3_TXADDR (0x180)
#define ENDP3_RXADDR (0x1C0)

48
flight/pios/stm32f10x/pios_i2c.c
View File

@ -404,8 +404,17 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter)
PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn);
PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn);
i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
// check for an empty read/write
if (i2c_adapter->active_txn->buf != NULL && i2c_adapter->active_txn->len != 0) {
// Data available
i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
} else {
// No Data available => Empty read/write
i2c_adapter->last_byte = NULL;
i2c_adapter->active_byte = i2c_adapter->last_byte + 1;
}
I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE);
if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) {
@ -924,9 +933,9 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
PIOS_Assert(valid)
if (!PIOS_I2C_validate(i2c_adapter)) {
return -1;
}
PIOS_DEBUG_Assert(txn_list);
PIOS_DEBUG_Assert(num_txns);
@ -968,6 +977,14 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
#endif
// Estimate bytes of transmission. Per txns: 1 adress byte + length
i2c_adapter->transfer_timeout_ticks = num_txns;
for (uint32_t i = 0; i < num_txns; i++) {
i2c_adapter->transfer_timeout_ticks += txn_list[i].len;
}
// timeout if it takes eight times the expected time
i2c_adapter->transfer_timeout_ticks <<= 3;
i2c_adapter->bus_error = false;
i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START);
@ -983,7 +1000,15 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
/* Spin waiting for the transfer to finish */
while (!i2c_adapter_fsm_terminated(i2c_adapter)) {
;
/* sleep 9 clock ticks (1 byte), because FSM can't be faster than one byte
FIXME: clock streching could make problems, but citing NPX: alsmost no slave device implements clock stretching
three times the expected time should cover clock delay */
PIOS_DELAY_WaituS(i2c_adapter->transfer_delay_uS);
i2c_adapter->transfer_timeout_ticks--;
if (i2c_adapter->transfer_timeout_ticks == 0) {
break;
}
}
if (i2c_adapter_wait_for_stopped(i2c_adapter)) {
@ -1010,9 +1035,9 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
PIOS_Assert(valid)
if (!PIOS_I2C_validate(i2c_adapter)) {
return;
}
uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);
@ -1136,9 +1161,10 @@ void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id)
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
if (!PIOS_I2C_validate(i2c_adapter)) {
return;
}
PIOS_Assert(valid)
#if defined(PIOS_I2C_DIAGNOSTICS)
uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);

2
flight/pios/stm32f4xx/inc/usb_conf.h
View File

@ -32,8 +32,6 @@
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx.h"
#include "usb_conf.h"
/** @addtogroup USB_OTG_DRIVER
* @{
*/

106
flight/pios/stm32f4xx/pios_i2c.c
View File

@ -94,26 +94,19 @@ static void go_bus_error(struct pios_i2c_adapter *i2c_adapter);
static void go_stopping(struct pios_i2c_adapter *i2c_adapter);
static void go_stopped(struct pios_i2c_adapter *i2c_adapter);
static void go_starting(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter);
static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter);
static void go_r_last_txn_pre_one(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter);
static void go_r_last_txn_pre_last(struct pios_i2c_adapter *i2c_adapter);
static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter);
static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter);
static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter);
static void go_w_more_txn_last(struct pios_i2c_adapter *i2c_adapter);
static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter);
static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter);
static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter);
static void go_nack(struct pios_i2c_adapter *i2c_adapter);
@ -414,8 +407,16 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter)
PIOS_DEBUG_Assert(i2c_adapter->active_txn >= i2c_adapter->first_txn);
PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn);
i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
// check for an empty read/write
if (i2c_adapter->active_txn->buf != NULL && i2c_adapter->active_txn->len != 0) {
// Data available
i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]);
i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]);
} else {
// No Data available => Empty read/write
i2c_adapter->last_byte = NULL;
i2c_adapter->active_byte = i2c_adapter->last_byte + 1;
}
I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE);
if (i2c_adapter->active_txn->rw == PIOS_I2C_TXN_READ) {
@ -753,6 +754,11 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter)
/* Initialize the I2C block */
I2C_Init(i2c_adapter->cfg->regs, (I2C_InitTypeDef *)&(i2c_adapter->cfg->init));
// for delays during transfer timeouts
// one tick correspond to transmission of 1 byte i.e. 9 clock ticks
i2c_adapter->transfer_delay_uS = 9000000 / (((I2C_InitTypeDef *)&(i2c_adapter->cfg->init))->I2C_ClockSpeed);
if (i2c_adapter->cfg->regs->SR2 & I2C_FLAG_BUSY) {
/* Reset the I2C block */
I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE);
@ -760,7 +766,6 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter)
}
}
#include <pios_i2c_priv.h>
/* Return true if the FSM is in a terminal state */
static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter)
@ -788,9 +793,18 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter)
xSemaphoreGive(i2c_adapter->sem_ready);
#endif /* USE_FREERTOS */
/* transfer_timeout_ticks is set by PIOS_I2C_Transfer_Callback */
/* Spin waiting for the transfer to finish */
while (!i2c_adapter_fsm_terminated(i2c_adapter)) {
;
// sleep 1 byte, as FSM can't be faster
// FIXME: clock stretching could make problems, but citing NPX: alsmost no slave device implements clock stretching
// three times the expected time should cover clock delay
PIOS_DELAY_WaituS(i2c_adapter->transfer_delay_uS);
i2c_adapter->transfer_timeout_ticks--;
if (i2c_adapter->transfer_timeout_ticks == 0) {
break;
}
}
if (i2c_adapter_wait_for_stopped(i2c_adapter)) {
@ -956,6 +970,7 @@ int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg)
NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->event.init));
NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->error.init));
/* No error */
return 0;
@ -967,9 +982,9 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
PIOS_Assert(valid)
if (!PIOS_I2C_validate(i2c_adapter)) {
return -1;
}
PIOS_DEBUG_Assert(txn_list);
PIOS_DEBUG_Assert(num_txns);
@ -1001,12 +1016,20 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
#ifdef USE_FREERTOS
/* Make sure the done/ready semaphore is consumed before we start */
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
#endif
// Estimate bytes of transmission. Per txns: 1 adress byte + length
i2c_adapter->transfer_timeout_ticks = num_txns;
for (uint32_t i = 0; i < num_txns; i++) {
i2c_adapter->transfer_timeout_ticks += txn_list[i].len;
}
// timeout if it takes eight times the expected time
i2c_adapter->transfer_timeout_ticks <<= 3;
i2c_adapter->callback = NULL;
i2c_adapter->bus_error = false;
i2c_adapter->nack = false;
i2c_adapter->nack = false;
i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START);
/* Wait for the transfer to complete */
@ -1017,7 +1040,15 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
/* Spin waiting for the transfer to finish */
while (!i2c_adapter_fsm_terminated(i2c_adapter)) {
;
/* sleep 9 clock ticks (1 byte), because FSM can't be faster than one byte
FIXME: clock stretching could make problems, but citing NPX: alsmost no slave device implements clock stretching
three times the expected time should cover clock delay */
PIOS_DELAY_WaituS(i2c_adapter->transfer_delay_uS);
i2c_adapter->transfer_timeout_ticks--;
if (i2c_adapter->transfer_timeout_ticks == 0) {
break;
}
}
if (i2c_adapter_wait_for_stopped(i2c_adapter)) {
@ -1048,9 +1079,10 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
if (!PIOS_I2C_validate(i2c_adapter)) {
return -1;
}
PIOS_Assert(valid)
PIOS_Assert(callback);
PIOS_DEBUG_Assert(txn_list);
@ -1080,9 +1112,18 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx
#ifdef USE_FREERTOS
/* Make sure the done/ready semaphore is consumed before we start */
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
#endif
// used in the i2c_adapter_callback_handler function
// Estimate bytes of transmission. Per txns: 1 adress byte + length
i2c_adapter->transfer_timeout_ticks = num_txns;
for (uint32_t i = 0; i < num_txns; i++) {
i2c_adapter->transfer_timeout_ticks += txn_list[i].len;
}
// timeout if it takes eight times the expected time
i2c_adapter->transfer_timeout_ticks <<= 3;
i2c_adapter->callback = callback;
i2c_adapter->bus_error = false;
i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START);
@ -1094,9 +1135,9 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
PIOS_Assert(valid)
if (!PIOS_I2C_validate(i2c_adapter)) {
return;
}
uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);
@ -1204,9 +1245,9 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
/* Ignore this event and wait for TRANSMITTED in case we can't keep up */
goto skip_event;
break;
case 0x30084: /* Occurs between byte tranmistted and master mode selected */
case 0x30000: /* Need to throw away this spurious event */
case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */
case 0x30084: /* BUSY + MSL + TXE + BFT Occurs between byte transmitted and master mode selected */
case 0x30000: /* BUSY + MSL Need to throw away this spurious event */
case 0x30403 & EVENT_MASK: /* BUSY + MSL + SB + ADDR Detected this after got a NACK, probably stop bit */
goto skip_event;
break;
default:
@ -1227,16 +1268,15 @@ void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id)
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
if (!PIOS_I2C_validate(i2c_adapter)) {
return;
}
PIOS_Assert(valid)
#if defined(PIOS_I2C_DIAGNOSTICS)
uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);
#if defined(PIOS_I2C_DIAGNOSTICS)
i2c_erirq_history[i2c_erirq_history_pointer] = event;
i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5;
#endif
if (event & I2C_FLAG_AF) {

2
flight/pios/stm32f4xx/pios_wdg.c
View File

@ -73,7 +73,7 @@ uint16_t PIOS_WDG_Init()
delay = 0x0fff;
}
#if defined(PIOS_INCLUDE_WDG)
DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); // make the watchdog stop counting in debug mode
DBGMCU_APB1PeriphConfig(DBGMCU_IWDG_STOP, ENABLE); // OP-1272 : write in APB1 register
IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
IWDG_SetPrescaler(IWDG_Prescaler_16);
IWDG_SetReload(delay);

1
flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h
View File

@ -31,6 +31,7 @@
#ifndef PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
// Note : changing below length will require changes to the USB buffer setup
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
#define PIOS_USB_BOARD_EP_NUM 2

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

@ -34,6 +34,7 @@ ERASE_FLASH ?= NO
MODULES += Attitude
MODULES += Stabilization
MODULES += Actuator
MODULES += Receiver
MODULES += ManualControl
MODULES += FirmwareIAP
MODULES += Telemetry
@ -70,7 +71,6 @@ ifndef TESTAPP
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/callbackscheduler.c
## UAVObjects
SRC += $(OPUAVSYNTHDIR)/accessorydesired.c
@ -84,16 +84,22 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/systemsettings.c
SRC += $(OPUAVSYNTHDIR)/stabilizationdesired.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c
SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c
SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
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
@ -108,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

17
flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h
View File

@ -23,7 +23,7 @@
/* Notes: We use 5 task priorities */
#define configUSE_PREEMPTION 1
#define configUSE_IDLE_HOOK 1
#define configUSE_IDLE_HOOK 0
#define configUSE_TICK_HOOK 0
#define configUSE_MALLOC_FAILED_HOOK 1
#define configCPU_CLOCK_HZ ((unsigned long)72000000)
@ -31,7 +31,7 @@
#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5)
#define configMINIMAL_STACK_SIZE ((unsigned short)48)
#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256))
#define configMAX_TASK_NAME_LEN (16)
#define configMAX_TASK_NAME_LEN (6)
#define configUSE_TRACE_FACILITY 0
#define configUSE_16_BIT_TICKS 0
#define configIDLE_SHOULD_YIELD 0
@ -39,7 +39,7 @@
#define configUSE_RECURSIVE_MUTEXES 1
#define configUSE_COUNTING_SEMAPHORES 0
#define configUSE_ALTERNATIVE_API 0
#define configQUEUE_REGISTRY_SIZE 10
#define configQUEUE_REGISTRY_SIZE 0
/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 0
@ -52,10 +52,10 @@
#define INCLUDE_uxTaskPriorityGet 1
#define INCLUDE_vTaskDelete 1
#define INCLUDE_vTaskCleanUpResources 0
#define INCLUDE_vTaskSuspend 1
#define INCLUDE_vTaskSuspend 0
#define INCLUDE_vTaskDelayUntil 1
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
#define INCLUDE_xTaskGetSchedulerState 0
#define INCLUDE_xTaskGetCurrentTaskHandle 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
@ -75,11 +75,9 @@
#endif
/* Enable run time stats collection */
#ifdef DIAG_TASKS
#define configCHECK_FOR_STACK_OVERFLOW 2
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
#define INCLUDE_xTaskGetIdleTaskHandle 1
#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \
do { \
(*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \
@ -87,6 +85,9 @@
} \
while (0)
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */
#ifdef DIAG_TASKS
#define configCHECK_FOR_STACK_OVERFLOW 2
#else
#define configCHECK_FOR_STACK_OVERFLOW 1
#endif

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

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

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

@ -43,6 +43,10 @@
/* PIOS FreeRTOS support */
#define PIOS_INCLUDE_FREERTOS
/* PIOS CallbackScheduler support */
#define PIOS_INCLUDE_CALLBACKSCHEDULER
/* PIOS bootloader helper */
#define PIOS_INCLUDE_BL_HELPER
/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */
@ -140,7 +144,7 @@
/* Stabilization options */
/* #define PIOS_QUATERNION_STABILIZATION */
#define PIOS_EXCLUDE_ADVANCED_FEATURES
/* Performance counters */
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998
@ -153,12 +157,19 @@
#define CPULOAD_LIMIT_CRITICAL 95
/* Task stack sizes */
#define PIOS_ACTUATOR_STACK_SIZE 1020
#define PIOS_MANUAL_STACK_SIZE 800
#define PIOS_ACTUATOR_STACK_SIZE 820
#define PIOS_MANUAL_STACK_SIZE 635
#define PIOS_RECEIVER_STACK_SIZE 620
#define PIOS_STABILIZATION_STACK_SIZE 780
#ifdef DIAG_TASKS
#define PIOS_SYSTEM_STACK_SIZE 740
#else
#define PIOS_SYSTEM_STACK_SIZE 660
#define PIOS_STABILIZATION_STACK_SIZE 524
#define PIOS_TELEM_STACK_SIZE 800
#define PIOS_EVENTDISPATCHER_STACK_SIZE 130
#endif
#define PIOS_TELEM_RX_STACK_SIZE 410
#define PIOS_TELEM_TX_STACK_SIZE 560
#define PIOS_EVENTDISPATCHER_STACK_SIZE 95
/* This can't be too high to stop eventdispatcher thread overflowing */
#define PIOS_EVENTDISAPTCHER_QUEUE 10

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

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

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

@ -31,6 +31,7 @@
#ifndef PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
// Note : changing below length will require changes to the USB buffer setup
#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64
#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64

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

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

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

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

1
flight/targets/boards/oplinkmini/bootloader/inc/pios_usb_board_data.h
View File

@ -31,6 +31,7 @@
#ifndef PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
// Note : changing below length will require changes to the USB buffer setup
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
#define PIOS_USB_BOARD_EP_NUM 2

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

@ -46,13 +46,13 @@ ifndef TESTAPP
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/callbackscheduler.c
## UAVObjects
SRC += $(OPUAVSYNTHDIR)/oplinkstatus.c
SRC += $(OPUAVSYNTHDIR)/oplinksettings.c
SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
SRC += $(OPUAVSYNTHDIR)/oplinkreceiver.c
SRC += $(OPUAVSYNTHDIR)/radiocombridgestats.c
else
## Test Code
SRC += $(OPTESTS)/test_common.c

7
flight/targets/boards/oplinkmini/firmware/inc/FreeRTOSConfig.h
View File

@ -75,11 +75,9 @@
#endif
/* Enable run time stats collection */
#ifdef DIAG_TASKS
#define configCHECK_FOR_STACK_OVERFLOW 2
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
#define INCLUDE_xTaskGetIdleTaskHandle 1
#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \
do { \
(*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \
@ -87,6 +85,9 @@
} \
while (0)
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */
#ifdef DIAG_TASKS
#define configCHECK_FOR_STACK_OVERFLOW 2
#else
#define configCHECK_FOR_STACK_OVERFLOW 1
#endif

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

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

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

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

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

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

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

@ -31,6 +31,7 @@
#ifndef PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
// Note : changing below length will require changes to the USB buffer setup
#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64
#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64

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

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

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

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

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