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

Merge remote-tracking branch 'origin/next' into thread/OP-1222_Fixed_Wing_Wizard_Support

Browse Source 
Getting more current code in place.
This commit is contained in:
Kevin Finisterre 2014-06-07 21:24:05 -04:00
commit c2c89bf0f5
1883 changed files with 234163 additions and 39937 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@ -24,6 +24,9 @@ core
# ground
openpilotgcs-build-desktop
ground/openpilotgcs/.cproject
ground/openpilotgcs/.project
ground/openpilotgcs/.settings
# Ignore some of the .pro.user files
*.pro.user

37
Makefile
View File

@ -197,7 +197,7 @@ export OPUAVSYNTHDIR := $(BUILD_DIR)/uavobject-synthetics/flight
export OPGCSSYNTHDIR := $(BUILD_DIR)/openpilotgcs-synthetics
# Define supported board lists
ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix
ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare
# Short names of each board (used to display board name in parallel builds)
coptercontrol_short := 'cc '
@ -206,6 +206,7 @@ revolution_short := 'revo'
osd_short := 'osd '
revoproto_short := 'revp'
simposix_short := 'posx'
discoveryf4bare_short := 'df4b'
# SimPosix only builds on Linux so drop it from the list for
# all other platforms.
@ -450,11 +451,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 ; \
)
@ -683,7 +698,7 @@ endif
##############################
# Firmware files to package
PACKAGE_FW_TARGETS := $(filter-out fw_simposix, $(FW_TARGETS))
PACKAGE_FW_TARGETS := $(filter-out fw_simposix fw_discoveryf4bare, $(FW_TARGETS))
PACKAGE_ELF_TARGETS := $(filter fw_simposix, $(FW_TARGETS))
# Rules to generate GCS resources used to embed firmware binaries into the GCS.
@ -709,7 +724,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
@ -732,7 +747,7 @@ ifneq ($(strip $(filter package clean_package,$(MAKECMDGOALS))),)
# Packaged GCS should depend on opfw_resource
ifneq ($(strip $(filter package clean_package,$(MAKECMDGOALS))),)
$(eval openpilotgcs: $(OPFW_RESOURCE))
$(eval openpilotgcs_qmake: $(OPFW_RESOURCE))
endif
# Clean the build directory if clean_package is requested
@ -864,9 +879,8 @@ help:
@$(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"
@ -947,6 +961,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)

128
WHATSNEW.txt
View File

@ -1,12 +1,5 @@
--- RELEASE-14.01-RC3 --- Cruising Ratt ---
this issue includes the following fixes to previous RC2:
OP-1088 OP-1141 OP-1166 OP-1187 OP-1191 OP-1195 OP-1211 OP-1218
Full list of bug fixed in this release is accessible here
http://progress.openpilot.org/issues/?filter=11361
--- RELEASE-14.01-RC2 --- Cruising Ratt ---
This is the RC2 for the first 2014 software release.
--- RELEASE-14.01 --- Cruising Ratt ---
This is the first 2014 software release.
This version still supports the CopterControl and CC3D.
It includes some major "under the hood" changes like migration
to Qt5.1 and QtQuick2 widgets, an overhaul of UAVTalk to improve
@ -20,12 +13,119 @@ Some additions in this release:
the full list of features, improvements and bufixes shipping
in this release is accessible here:
http://progress.openpilot.org/browse/OP/fixforversion/10220
http://progress.openpilot.org/issues/?filter=11260
Issues fixes since RC1 release
http://progress.openpilot.org/issues/?jql=labels%20%3D%20%2214.01-rc1%22
OP-1166 OP-1168 OP-1169 OP-1176 OP-1177 OP-1178 OP-1179 OP-1180
OP-1182 OP-1183 OP-1184 OP-1187 OP-1188 OP-1192
** 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.

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

65
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 (((uint16_t)(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 (((uint16_t)(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;
}
@ -240,6 +254,33 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
xSemaphoreGiveRecursive(lock);
return 0;
}
/**
* Get the highest alarm severity
* @return
*/
SystemAlarmsAlarmOptions AlarmsGetHighestSeverity()
{
SystemAlarmsAlarmData alarmsData;
SystemAlarmsAlarmOptions highest = SYSTEMALARMS_ALARM_UNINITIALISED;
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarms
SystemAlarmsAlarmGet(&alarmsData);
// Go through alarms and find the highest severity
uint32_t n = 0;
while (n < SYSTEMALARMS_ALARM_NUMELEM && highest != SYSTEMALARMS_ALARM_CRITICAL) {
if (cast_struct_to_array(alarmsData, alarmsData.Actuator)[n] > highest) {
highest = cast_struct_to_array(alarmsData, alarmsData.Actuator)[n];
}
n++;
}
xSemaphoreGiveRecursive(lock);
return highest;
}
/**
* @}

22
flight/libraries/inc/CoordinateConversions.h
View File

@ -75,5 +75,27 @@ void quat_inverse(float q[4]);
void quat_copy(const float q[4], float qnew[4]);
void quat_mult(const float q1[4], const float q2[4], float qout[4]);
void rot_mult(float R[3][3], const float vec[3], float vec_out[3]);
/**
* matrix_mult_3x3f - perform a multiplication between two 3x3 float matrices
* result = a*b
* @param a
* @param b
* @param result
*/
inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3])
{
result[0][0] = a[0][0] * b[0][0] + a[1][0] * b[0][1] + a[2][0] * b[0][2];
result[0][1] = a[0][1] * b[0][0] + a[1][1] * b[0][1] + a[2][1] * b[0][2];
result[0][2] = a[0][2] * b[0][0] + a[1][2] * b[0][1] + a[2][2] * b[0][2];
result[1][0] = a[0][0] * b[1][0] + a[1][0] * b[1][1] + a[2][0] * b[1][2];
result[1][1] = a[0][1] * b[1][0] + a[1][1] * b[1][1] + a[2][1] * b[1][2];
result[1][2] = a[0][2] * b[1][0] + a[1][2] * b[1][1] + a[2][2] * b[1][2];
result[2][0] = a[0][0] * b[2][0] + a[1][0] * b[2][1] + a[2][0] * b[2][2];
result[2][1] = a[0][1] * b[2][0] + a[1][1] * b[2][1] + a[2][1] * b[2][2];
result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
}
#endif // COORDINATECONVERSIONS_H_

2
flight/libraries/inc/alarms.h
View File

@ -42,9 +42,11 @@ int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm);
void AlarmsDefaultAll();
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm);
void AlarmsClearAll();
int32_t AlarmsHasWarnings();
int32_t AlarmsHasErrors();
int32_t AlarmsHasCritical();
SystemAlarmsAlarmOptions AlarmsGetHighestSeverity();
#endif // ALARMS_H

1
flight/libraries/inc/insgps.h
View File

@ -67,6 +67,7 @@ void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);
void INSSetGyroBiasVar(float gyro_bias_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetG(float g_e);
void INSSetMagVar(float scaled_mag_var[3]);
void INSSetBaroVar(float baro_var);
void INSPosVelReset(float pos[3], float vel[3]);

38
flight/libraries/inc/notification.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
*
* @file notification.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief notification library
* --
* @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 NOTIFICATION_H
#define NOTIFICATION_H
// period of each blink phase
#define LED_BLINK_PERIOD_MS 200
// update the status snapshot used by notifcations
void NotificationUpdateStatus();
// run the led notifications
void NotificationOnboardLedsRun();
#endif /* NOTIFICATION_H */

69
flight/libraries/inc/plans.h Normal file
View File

@ -0,0 +1,69 @@
/**
******************************************************************************
* @addtogroup OpenPilotLibraries OpenPilot Libraries
* @{
* @addtogroup Navigation
* @brief setups RTH/PH and other pathfollower/pathplanner status
* @{
*
* @file plan.h
* @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
*/
#ifndef PLANS_H_
#define PLANS_H_
#include <pios_math.h>
/** \page standard Plans
How to use this library
\li Call plan_initialize() to init all needed struct and uavos at startup.<br>
It may be safely called more than once.<br>
\li Functions called plan_setup_* needs to be called once, every time the requested function is engaged.<br>
\li Functions called plan_run_* are to be periodically called while the requested mode is engaged.<br>
*/
/**
* @brief initialize UAVOs and structs used by this library
*/
void plan_initialize();
/**
* @brief setup pathplanner/pathfollower for positionhold
*/
void plan_setup_positionHold();
/**
* @brief setup pathplanner/pathfollower for return to base
*/
void plan_setup_returnToBase();
/**
* @brief setup pathplanner/pathfollower for land
*/
void plan_setup_land();
/**
* @brief execute land
*/
void plan_run_land();
#endif /* PLANS_H_ */

9
flight/libraries/insgps13state.c
View File

@ -91,6 +91,8 @@ static struct EKFData {
float H[NUMV][NUMX];
// local magnetic unit vector in NED frame
float Be[3];
// local gravity vector
float g_e;
// covariance matrix and state vector
float P[NUMX][NUMX];
float X[NUMX];
@ -286,6 +288,11 @@ void INSSetMagNorth(float B[3])
ekf.Be[2] = B[2] / mag;
}
void INSSetG(float g_e)
{
ekf.g_e = g_e;
}
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
{
float U[6];
@ -641,7 +648,7 @@ void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
az;
Xdot[5] =
2.0f * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81f;
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + ekf.g_e;
// qdot = Q*w
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f;

17
flight/modules/AltitudeHold/inc/altitudehold.h → flight/libraries/math/mathmisc.c
View File

@ -1,9 +1,13 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Reuseable math functions
* @{
*
* @file examplemodperiodic.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Example module to be used as a template for actual modules.
* @file mathmisc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Reuseable math functions
*
* @see The GNU Public License (GPL) Version 3
*
@ -23,9 +27,6 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef EXAMPLEMODPERIODIC_H
#define EXAMPLEMODPERIODIC_H
int32_t ExampleModPeriodicInitialize();
int32_t GuidanceInitialize(void);
#endif // EXAMPLEMODPERIODIC_H
// space deliberately left empty, any non inline misc math functions can go here

55
flight/libraries/math/mathmisc.h Normal file
View File

@ -0,0 +1,55 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Reuseable math functions
* @{
*
* @file mathmisc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Reuseable math functions
*
* @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 MATHMISC_H
#define MATHMISC_H
// returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
// returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)
static inline float boundf(float val, float boundary1, float boundary2)
{
if (boundary1 > boundary2) {
if (!(val >= boundary2)) {
return boundary2;
} else if (!(val <= boundary1)) {
return boundary1;
}
} else {
if (!(val >= boundary1)) {
return boundary1;
} else if (!(val <= boundary2)) {
return boundary2;
}
}
return val;
}
#endif /* MATHMISC_H */

21
flight/libraries/math/pid.c
View File

@ -30,11 +30,9 @@
#include "openpilot.h"
#include "pid.h"
#include <mathmisc.h>
#include <pios_math.h>
// ! Private method
static float bound(float val, float range);
// ! Store the shared time constant for the derivative cutoff.
static float deriv_tau = 7.9577e-3f;
@ -52,7 +50,7 @@ float pid_apply(struct pid *pid, const float err, float dT)
{
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term
float diff = (err - pid->lastErr);
@ -84,7 +82,7 @@ float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoi
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term,
float dterm = 0;
@ -142,16 +140,3 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
pid->d = d;
pid->iLim = iLim;
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if (val < -range) {
val = -range;
} else if (val > range) {
val = range;
}
return val;
}

296
flight/libraries/notification.c Normal file
View File

@ -0,0 +1,296 @@
/**
******************************************************************************
*
* @file notification.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief notification library.
* --
* @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/notification.h"
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <systemalarms.h>
#include <flightstatus.h>
#include <pios_notify.h>
#ifdef PIOS_LED_ALARM
#define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM)
#define ALARM_LED_OFF() PIOS_LED_Off(PIOS_LED_ALARM)
#else
#define ALARM_LED_ON()
#define ALARM_LED_OFF()
#endif
#ifdef PIOS_LED_HEARTBEAT
#define HEARTBEAT_LED_ON() PIOS_LED_On(PIOS_LED_HEARTBEAT)
#define HEARTBEAT_LED_OFF() PIOS_LED_Off(PIOS_LED_HEARTBEAT)
#else
#define HEARTBEAT_LED_ON()
#define HEARTBEAT_LED_OFF()
#endif
#define BLINK_R_ALARM_PATTERN(x) \
(x == SYSTEMALARMS_ALARM_OK ? 0 : \
x == SYSTEMALARMS_ALARM_WARNING ? 0b0000000001000000 : \
x == SYSTEMALARMS_ALARM_ERROR ? 0b0000001000100000 : \
x == SYSTEMALARMS_ALARM_CRITICAL ? 0b0111111111111110 : 0)
#define BLINK_B_ALARM_PATTERN(x) \
(x == SYSTEMALARMS_ALARM_OK ? 0 : \
x == SYSTEMALARMS_ALARM_WARNING ? 0 : \
x == SYSTEMALARMS_ALARM_ERROR ? 0 : \
x == SYSTEMALARMS_ALARM_CRITICAL ? 0 : 0)
#define BLINK_B_FM_ARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100010001 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100010001 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000100001 : 0b0000000000000001)
#define BLINK_R_FM_ARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000000000 : 0b0000010000000000)
#define BLINK_B_FM_DISARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000110001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000110001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100110011 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100110011 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110001100011 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110001100011 : 0b0000000000000011)
#define BLINK_R_FM_DISARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000011 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000011 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110000000000 : 0b0000110000000000)
#define BLINK_B_HEARTBEAT_PATTERN 0b0001111111111111
#define BLINK_R_HEARTBEAT_PATTERN 0
#define BLINK_B_NOTIFY_PATTERN(x) \
(x == NOTIFY_NONE ? 0 : \
x == NOTIFY_OK ? 0b0000100100111111 : \
x == NOTIFY_NOK ? 0b0000000000111111 : \
x == NOTIFY_DRAW_ATTENTION ? 0b0101010101010101 : 0b0101010101010101)
#define BLINK_R_NOTIFY_PATTERN(x) \
(x == NOTIFY_NONE ? 0 : \
x == NOTIFY_OK ? 0b0000000000001111 : \
x == NOTIFY_NOK ? 0b0011000011001111 : \
x == NOTIFY_DRAW_ATTENTION ? 0b1010101010101010 : 0b1010101010101010)
// led notification handling
static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK;
static volatile FlightStatusData currentFlightStatus;
static volatile bool started = false;
static volatile pios_notify_notification nextNotification = NOTIFY_NONE;
#ifdef PIOS_LED_ALARM
static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern);
#endif // PIOS_LED_ALARM
static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern);
static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern);
static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern);
void NotificationUpdateStatus()
{
started = true;
// get values to be used for led handling
FlightStatusGet((FlightStatusData *)&currentFlightStatus);
currentAlarmLevel = AlarmsGetHighestSeverity();
if (nextNotification == NOTIFY_NONE) {
nextNotification = PIOS_NOTIFY_GetActiveNotification(true);
}
}
void NotificationOnboardLedsRun()
{
static portTickType lastRunTimestamp;
static uint16_t r_pattern;
static uint16_t b_pattern;
static uint8_t cycleCount; // count the number of cycles
static uint8_t lastFlightMode = -1;
static bool forceShowFlightMode = false;
static pios_notify_notification runningNotification = NOTIFY_NONE;
static enum {
STATUS_NOTIFY,
STATUS_ALARM,
STATUS_FLIGHTMODE, // flightMode/HeartBeat
STATUS_LENGHT
} status;
if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 4)) {
return;
}
lastRunTimestamp = xTaskGetTickCount();
// the led will show various status information, subdivided in three phases
// - Notification
// - Alarm
// - Flight status
// they are shown using the above priority
// a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end
cycleCount++;
// Notifications are "modal" to other statuses so they takes precedence
if (status != STATUS_NOTIFY && nextNotification != NOTIFY_NONE) {
// read next notification to show
runningNotification = nextNotification;
nextNotification = NOTIFY_NONE;
// Force a notification status
status = STATUS_NOTIFY;
cycleCount = 0; // instantly start a notify cycle
} else {
if (lastFlightMode != currentFlightStatus.FlightMode) {
status = STATUS_FLIGHTMODE;
lastFlightMode = currentFlightStatus.FlightMode;
cycleCount = 0; // instantly start a flightMode cycle
forceShowFlightMode = true;
}
}
// check if a phase has just finished
if (cycleCount & 0x10) {
HEARTBEAT_LED_OFF();
ALARM_LED_OFF();
cycleCount = 0x0;
forceShowFlightMode = false;
// Notification has been just shown, cleanup
if (status == STATUS_NOTIFY) {
runningNotification = NOTIFY_NONE;
}
status = (status + 1) % STATUS_LENGHT;
}
if (status == STATUS_NOTIFY) {
if (!cycleCount && !handleNotifications(runningNotification, &r_pattern, &b_pattern)) {
// no notifications, advance
status++;
}
}
// Handles Alarm display
if (status == STATUS_ALARM) {
#ifdef PIOS_LED_ALARM
if (!cycleCount && !handleAlarms(&r_pattern, &b_pattern)) {
// no alarms, advance
status++;
}
#else
// no alarms leds, advance
status++;
#endif // PIOS_LED_ALARM
}
// **** Handles flightmode display
if (status == STATUS_FLIGHTMODE && !cycleCount) {
if (forceShowFlightMode || currentFlightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
handleFlightMode(&r_pattern, &b_pattern);
} else {
handleHeartbeat(&r_pattern, &b_pattern);
}
}
// led output
if (b_pattern & 0x1) {
HEARTBEAT_LED_ON();
} else {
HEARTBEAT_LED_OFF();
}
if (r_pattern & 0x1) {
ALARM_LED_ON();
} else {
ALARM_LED_OFF();
}
r_pattern >>= 1;
b_pattern >>= 1;
}
#if defined(PIOS_LED_ALARM)
static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern)
{
if (currentAlarmLevel == SYSTEMALARMS_ALARM_OK) {
return false;
}
*b_pattern = BLINK_B_ALARM_PATTERN(currentAlarmLevel);
*r_pattern = BLINK_R_ALARM_PATTERN(currentAlarmLevel);
return true;
}
#endif /* PIOS_LED_ALARM */
static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern)
{
if (runningNotification == NOTIFY_NONE) {
return false;
}
*b_pattern = BLINK_B_NOTIFY_PATTERN(runningNotification);
*r_pattern = BLINK_R_NOTIFY_PATTERN(runningNotification);
return true;
}
static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern)
{
// Flash the heartbeat LED
uint8_t flightmode = currentFlightStatus.FlightMode;
if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
*b_pattern = BLINK_B_FM_DISARMED_PATTERN(flightmode);
*r_pattern = BLINK_R_FM_DISARMED_PATTERN(flightmode);
} else {
*b_pattern = BLINK_B_FM_ARMED_PATTERN(flightmode);
*r_pattern = BLINK_R_FM_ARMED_PATTERN(flightmode);
}
}
static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern)
{
// Flash the heartbeat LED
*b_pattern = BLINK_B_HEARTBEAT_PATTERN;
*r_pattern = BLINK_R_HEARTBEAT_PATTERN;
}

130
flight/libraries/plans.c Normal file
View File

@ -0,0 +1,130 @@
/**
******************************************************************************
* @addtogroup OpenPilotLibraries OpenPilot Libraries
* @{
* @addtogroup Navigation
* @brief setups RTH/PH and other pathfollower/pathplanner status
* @{
*
* @file plan.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 <plans.h>
#include <openpilot.h>
#include <attitudesettings.h>
#include <takeofflocation.h>
#include <pathdesired.h>
#include <positionstate.h>
#include <flightmodesettings.h>
/**
* @brief initialize UAVOs and structs used by this library
*/
void plan_initialize()
{
TakeOffLocationInitialize();
PositionStateInitialize();
PathDesiredInitialize();
FlightModeSettingsInitialize();
}
/**
* @brief setup pathplanner/pathfollower for positionhold
*/
void plan_setup_positionHold()
{
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);
}
/**
* @brief setup pathplanner/pathfollower for return to base
*/
void plan_setup_returnToBase()
{
// Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
float positionStateDown;
PositionStateDownGet(&positionStateDown);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
TakeOffLocationData takeoffLocation;
TakeOffLocationGet(&takeoffLocation);
// TODO: right now VTOLPF does fly straight to destination altitude.
// For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
float destDown;
FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown);
destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
pathDesired.Start.North = takeoffLocation.North;
pathDesired.Start.East = takeoffLocation.East;
pathDesired.Start.Down = destDown;
pathDesired.End.North = takeoffLocation.North;
pathDesired.End.East = takeoffLocation.East;
pathDesired.End.Down = destDown;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
void plan_setup_land()
{
plan_setup_positionHold();
}
/**
* @brief execute land
*/
void plan_run_land()
{
PathDesiredEndData pathDesiredEnd;
PathDesiredEndGet(&pathDesiredEnd);
PositionStateDownGet(&pathDesiredEnd.Down);
pathDesiredEnd.Down += 5;
PathDesiredEndSet(&pathDesiredEnd);
}

211
flight/libraries/sanitycheck.c
View File

@ -34,10 +34,16 @@
// UAVOs
#include <manualcontrolsettings.h>
#include <flightmodesettings.h>
#include <systemsettings.h>
#include <systemalarms.h>
#include <revosettings.h>
#include <taskinfo.h>
// a number of useful macros
#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_ERROR))
/****************************
* Current checks:
* 1. If a flight mode switch allows autotune and autotune module not running
@ -45,7 +51,7 @@
****************************/
// ! Check a stabilization mode switch position for safety
static int32_t check_stabilization_settings(int index, bool multirotor);
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol);
/**
* Run a preflight check over the hardware configuration
@ -60,8 +66,27 @@ int32_t configuration_check()
const struct pios_board_info *bdinfo = &pios_board_info_blob;
bool coptercontrol = bdinfo->board_type == 0x04;
// Classify navigation capability
#ifdef REVOLUTION
RevoSettingsInitialize();
uint8_t revoFusion;
RevoSettingsFusionAlgorithmGet(&revoFusion);
bool navCapableFusion;
switch (revoFusion) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
navCapableFusion = true;
break;
default:
navCapableFusion = false;
}
#else
const bool navCapableFusion = false;
#endif
// Classify airframe type
bool multirotor = true;
bool multirotor;
uint8_t airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type);
@ -85,95 +110,57 @@ 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:
if (multirotor) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
ADDSEVERITY(!multirotor);
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol));
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol));
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol));
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol));
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
// TODO: put check equivalent to TASK_MONITOR_IsRunning
// here as soon as available for delayed callbacks
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol));
break;
case MANUALCONTROLSETTINGS_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;
}
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol));
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports Position Hold
severity = SYSTEMALARMS_ALARM_ERROR;
}
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE));
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports AutoLand Mode
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports POI Mode
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
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:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports ReturnToBase
severity = SYSTEMALARMS_ALARM_ERROR;
}
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
{
// 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);
ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK);
}
// intentionally no break as this also needs pathfollower
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER));
ADDSEVERITY(navCapableFusion);
break;
default:
// Uncovered modes are automatically an error
severity = SYSTEMALARMS_ALARM_ERROR;
ADDSEVERITY(false);
}
// mark the first encountered erroneous setting in status and substatus
if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
@ -196,45 +183,75 @@ int32_t configuration_check()
* Checks the stabiliation settings for a paritcular mode and makes
* sure it is appropriate for the airframe
* @param[in] index Which stabilization mode to check
* @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_ERROR
* @returns true or false
*/
static int32_t check_stabilization_settings(int index, bool multirotor)
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol)
{
// Make sure the modes have identical sizes
if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_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;
case 4:
FlightModeSettingsStabilization4SettingsArrayGet(modes);
break;
case 5:
FlightModeSettingsStabilization5SettingsArrayGet(modes);
break;
case 6:
FlightModeSettingsStabilization6SettingsArrayGet(modes);
break;
default:
return SYSTEMALARMS_ALARM_ERROR;
return false;
}
// For multirotors verify that nothing is set to "none"
// For multirotors verify that roll/pitch/yaw are not set to "none"
// (why not? might be fun to test ones reactions ;) if you dare, set your frame to "custom"!
if (multirotor) {
for (uint32_t i = 0; i < NELEMENTS(modes); i++) {
if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) {
return SYSTEMALARMS_ALARM_ERROR;
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
return false;
}
}
}
// Warning: This assumes that certain conditions in the XML file are met. That
// MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE
// coptercontrol cannot do altitude holding
if (coptercontrol) {
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
) {
return false;
}
}
return SYSTEMALARMS_ALARM_OK;
// check that thrust modes are only set to thrust axis
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
) {
return false;
}
}
if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
)) {
return false;
}
// Warning: This assumes that certain conditions in the XML file are met. That
// FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
// (this is checked at compile time by static constraint manualcontrol.h)
return true;
}

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

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

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

@ -41,6 +41,7 @@
#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"
@ -48,7 +49,7 @@
// Private constants
#define STACK_SIZE_BYTES 500
#define STACK_SIZE_BYTES 650
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
@ -59,7 +60,7 @@
static xTaskHandle taskHandle;
static bool airspeedEnabled = false;
static AirspeedSettingsData airspeedSettings;
static AirspeedSettingsAirspeedSensorTypeOptions lastAirspeedSensorType = -1;
static int8_t airspeedADCPin = -1;
@ -134,14 +135,12 @@ MODULE_INITCALL(AirspeedInitialize, AirspeedStart);
static void airspeedTask(__attribute__((unused)) void *parameters)
{
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
bool gpsAirspeedInitialized = false;
AirspeedSensorData airspeedData;
AirspeedSensorGet(&airspeedData);
AirspeedSettingsUpdatedCb(NULL);
gps_airspeedInitialize();
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
// Main task loop
@ -152,6 +151,25 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
// Update the airspeed object
AirspeedSensorGet(&airspeedData);
// if sensor type changed reset Airspeed alarm
if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) {
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_DEFAULT);
lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
switch (airspeedSettings.AirspeedSensorType) {
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
// AirspeedSensor will not be updated until a different sensor is selected
// set the disconencted satus now
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedSensorSet(&airspeedData);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
if (!gpsAirspeedInitialized) {
gpsAirspeedInitialized = true;
gps_airspeedInitialize();
}
break;
}
}
switch (airspeedSettings.AirspeedSensorType) {
#if defined(PIOS_INCLUDE_MPXV)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
@ -165,12 +183,24 @@ 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:
// no need to check so often until a sensor is enabled
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_DEFAULT);
vTaskDelay(2000 / portTICK_RATE_MS);
continue;
default:
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
break;
}
// Set the UAVO

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

@ -64,11 +64,13 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
if (airspeedSensor->SensorValue == (uint16_t)-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
return;
}
// only calibrate if no stored calibration is available
if (!airspeedSettings->ZeroPoint) {
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
// Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
@ -93,6 +95,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;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
}

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

@ -63,7 +63,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
// Ensure that the ADC pin is properly configured
if (airspeedADCPin < 0) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
return;
}
if (sensor.type == PIOS_MPXV_UNKNOWN) {
@ -76,6 +76,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
break;
default:
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
return;
}
}
@ -83,6 +84,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor);
if (!airspeedSettings->ZeroPoint) {
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, 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 +109,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;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
}
#endif /* if defined(PIOS_INCLUDE_MPXV) */

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

@ -0,0 +1,139 @@
/**
******************************************************************************
* @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 "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;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, 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);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, 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;
}
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, 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
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
}
#endif /* if defined(PIOS_INCLUDE_MS4525DO) */
/**
* @}
* @}
*/

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

@ -45,6 +45,7 @@
#define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO
#define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO
#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO
#define COSINE_OF_5_DEG 0.9961947f
// Private types
struct GPSGlobals {
@ -59,6 +60,12 @@ struct GPSGlobals {
static struct GPSGlobals *gps;
// Private functions
// a simple square inline function based on multiplication faster than powf(x,2.0f)
static inline float Sq(float x)
{
return x * x;
}
/*
* Initialize function loads first data sets, and allocates memory for structure.
@ -102,6 +109,11 @@ void gps_airspeedInitialize()
* where "f" is the fuselage vector in earth coordinates.
* We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
*/
/* Remark regarding "IMU Wind Estimation": The approach includes errors when |V| is
* not constant, i.e. when the change in V_gps does not solely come from a reorientation
* this error depends strongly on the time scale considered. Is the time between t1 and t2 too
* small, "spikes" absorving unconsidred acceleration will arise
*/
void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings)
{
float Rbe[3][3];
@ -120,32 +132,39 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air
float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]);
// If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
if (fabsf(cosDiff) < COSINE_OF_5_DEG) {
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
return; // do not calculate if gps velocity is insufficient...
}
// Calculate the norm^2 of the difference between the two GPS vectors
float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f);
float normDiffGPS2 = Sq(gpsVelData.North - gps->gpsVelOld_N) + Sq(gpsVelData.East - gps->gpsVelOld_E) + Sq(gpsVelData.Down - gps->gpsVelOld_D);
// Calculate the norm^2 of the difference between the two fuselage vectors
float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f);
float normDiffAttitude2 = Sq(Rbe[0][0] - gps->RbeCol1_old[0]) + Sq(Rbe[0][1] - gps->RbeCol1_old[1]) + Sq(Rbe[0][2] - gps->RbeCol1_old[2]);
// Airspeed magnitude is the ratio between the two difference norms
float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2);
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
} else if (!IS_REAL(airspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
} 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;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
}
// Save old variables for next pass

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

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

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

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

@ -1,203 +0,0 @@
/**
******************************************************************************
*
* @file guidance.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @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
*/
/**
* Input object: ActiveWaypoint
* Input object: PositionState
* Input object: ManualControlCommand
* Output object: AttitudeDesired
*
* This module will periodically update the value of the AttitudeDesired object.
*
* The module executes in its own thread in this example.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include <openpilot.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <altitudeholdsettings.h>
#include <altitudeholddesired.h> // object that will be updated by the module
#include <altitudeholdstatus.h>
#include <flightstatus.h>
#include <stabilizationdesired.h>
#include <accelstate.h>
#include <pios_constants.h>
#include <velocitystate.h>
#include <positionstate.h>
// Private constants
#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 DelayedCallbackInfo *altitudeHoldCBInfo;
static AltitudeHoldSettingsData altitudeHoldSettings;
static struct pid pid0, pid1;
// Private functions
static void altitudeHoldTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeHoldStart()
{
// Start main task
SettingsUpdatedCb(NULL);
DelayedCallbackDispatch(altitudeHoldCBInfo);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeHoldInitialize()
{
AltitudeHoldSettingsInitialize();
AltitudeHoldDesiredInitialize();
AltitudeHoldStatusInitialize();
// Create object queue
altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
return 0;
}
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void)
{
static float startThrottle = 0.5f;
// make sure we run only when we are supposed to run
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
break;
default:
pid_zero(&pid0);
pid_zero(&pid1);
StabilizationDesiredThrottleGet(&startThrottle);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
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 throttle;
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE:
throttle = altitudeHoldDesired.SetPoint;
break;
default:
// velocity control loop
throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
if (throttle >= 1.0f) {
throttle = 1.0f;
}
if (throttle <= 0.0f) {
throttle = 0.0f;
}
break;
}
StabilizationDesiredData stab;
StabilizationDesiredGet(&stab);
stab.Roll = altitudeHoldDesired.Roll;
stab.Pitch = altitudeHoldDesired.Pitch;
stab.Yaw = altitudeHoldDesired.Yaw;
stab.Throttle = throttle;
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stab);
DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
}
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);
}

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

@ -57,12 +57,13 @@
#include "accelstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "accelgyrosettings.h"
#include "flightstatus.h"
#include "manualcontrolcommand.h"
#include "taskinfo.h"
#include "CoordinateConversions.h"
#include <pios_notify.h>
// Private constants
#define STACK_SIZE_BYTES 540
@ -70,7 +71,6 @@
#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
@ -102,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
@ -142,6 +160,7 @@ int32_t AttitudeInitialize(void)
{
AttitudeStateInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
AccelStateInitialize();
GyroStateInitialize();
@ -172,7 +191,7 @@ int32_t AttitudeInitialize(void)
trim_requested = false;
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -196,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)
@ -235,6 +252,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
rollPitchBiasRate = 0.01f;
accel_filter_enabled = false;
init = 0;
PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
} else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1.0f;
accelKi = 0.0f;
@ -242,6 +260,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
rollPitchBiasRate = 0.01f;
accel_filter_enabled = false;
init = 0;
PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
@ -280,8 +299,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
@ -309,9 +326,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;
@ -325,9 +342,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
@ -365,9 +383,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
@ -395,7 +413,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];
@ -411,15 +429,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) */
@ -437,9 +470,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];
@ -590,14 +623,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;
@ -612,17 +645,59 @@ 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 &&
attitudeSettings.BoardRotation.Yaw == 0) {
if (fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f &&
fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f &&
fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) {
rotate = 0;
// Shouldn't be used but to be safe
@ -646,11 +721,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];

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

@ -181,9 +181,10 @@ static void AutotuneTask(__attribute__((unused)) void *parameters)
stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
stabDesired.Throttle = manualControl.Throttle;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_THRUST] = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.Thrust = manualControl.Thrust;
switch (state) {
case AT_INIT:
@ -191,7 +192,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 +237,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]);
}
}

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

@ -79,7 +79,6 @@ static struct CameraStab_data {
// Private functions
static void attitudeUpdated(UAVObjEvent *ev);
static float bound(float val, float limit);
#ifdef USE_GIMBAL_FF
static void applyFeedForward(uint8_t index, float dT, float *attitude, CameraStabSettingsData *cameraStab);
@ -128,6 +127,7 @@ int32_t CameraStabInitialize(void)
.obj = AttitudeStateHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
@ -185,8 +185,9 @@ static void attitudeUpdated(UAVObjEvent *ev)
input_rate = accessory.AccessoryVal *
cast_struct_to_array(cameraStab.InputRate, cameraStab.InputRate.Roll)[i];
if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) {
csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis,
cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]);
csd->inputs[i] = boundf(csd->inputs[i] + input_rate * 0.001f * dT_millis,
-cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i],
cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]);
}
break;
default:
@ -228,7 +229,7 @@ static void attitudeUpdated(UAVObjEvent *ev)
// bounding for elevon mixing occurs on the unmixed output
// to limit the range of the mixed output you must limit the range
// of both the unmixed pitch and unmixed roll
float output = bound((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], 1.0f);
float output = boundf((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], -1.0f, 1.0f);
// set output channels
switch (i) {
@ -281,13 +282,6 @@ static void attitudeUpdated(UAVObjEvent *ev)
}
}
float bound(float val, float limit)
{
return (val > limit) ? limit :
(val < -limit) ? -limit :
val;
}
#ifdef USE_GIMBAL_FF
void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraStabSettingsData *cameraStab)
{

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

515
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"
@ -64,6 +63,7 @@
#include "taskinfo.h"
#include <pios_struct_helper.h>
#include "sin_lookup.h"
#include "paths.h"
#include "CoordinateConversions.h"
@ -86,7 +86,7 @@ static void updatePathVelocity();
static uint8_t updateFixedDesiredAttitude();
static void updateFixedAttitude();
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
static float bound(float val, float min, float max);
static bool correctCourse(float *C, float *V, float *F, float s);
/**
* Initialise the module, called on startup
@ -127,17 +127,17 @@ int32_t FixedWingPathFollowerInitialize()
}
MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart);
static float northVelIntegral = 0;
static float eastVelIntegral = 0;
static float downVelIntegral = 0;
static float northVelIntegral = 0.0f;
static float eastVelIntegral = 0.0f;
static float downVelIntegral = 0.0f;
static float courseIntegral = 0;
static float speedIntegral = 0;
static float powerIntegral = 0;
static float airspeedErrorInt = 0;
static float courseIntegral = 0.0f;
static float speedIntegral = 0.0f;
static float powerIntegral = 0.0f;
static float airspeedErrorInt = 0.0f;
// correct speed by measured airspeed
static float indicatedAirspeedStateBias = 0;
static float indicatedAirspeedStateBias = 0.0f;
/**
* Module thread, should not return.
@ -182,61 +182,57 @@ 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;
downVelIntegral = 0;
courseIntegral = 0;
speedIntegral = 0;
powerIntegral = 0;
break;
northVelIntegral = 0.0f;
eastVelIntegral = 0.0f;
downVelIntegral = 0.0f;
courseIntegral = 0.0f;
speedIntegral = 0.0f;
powerIntegral = 0.0f;
}
PathStatusSet(&pathStatus);
}
@ -282,14 +278,14 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress, 0, 1);
boundf(progress.fractional_progress, 0.0f, 1.0f);
altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) *
bound(progress.fractional_progress, 0, 1);
boundf(progress.fractional_progress, 0.0f, 1.0f);
break;
}
// make sure groundspeed is not zero
if (groundspeed < 1e-2f) {
groundspeed = 1e-2f;
if (groundspeed < 1e-6f) {
groundspeed = 1e-6f;
}
// calculate velocity - can be zero if waypoints are too close
@ -308,24 +304,12 @@ static void updatePathVelocity()
// in this case the plane effectively needs to be turned around
// indicators:
// difference between correction_direction and velocitystate >90 degrees and
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from eerything )
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything )
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityState.East, velocityState.North));
float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityState.East, velocityState.North));
if (angle1 < -180.0f) {
angle1 += 360.0f;
}
if (angle1 > 180.0f) {
angle1 -= 360.0f;
}
if (angle2 < -180.0f) {
angle2 += 360.0f;
}
if (angle2 > 180.0f) {
angle2 -= 360.0f;
}
if (fabsf(angle1) >= 90.0f && fabsf(angle2) >= 90.0f) {
error_speed = 0;
if ( // calculating angles < 90 degrees through dot products
((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East) < 0.0f) &&
((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East) < 0.0f)) {
error_speed = 0.0f;
}
// calculate correction - can also be zero if correction vector is 0 or no error present
@ -334,14 +318,16 @@ static void updatePathVelocity()
// scale to correct length
float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
velocityDesired.North *= groundspeed / l;
velocityDesired.East *= groundspeed / l;
if (l > 1e-9f) {
velocityDesired.North *= groundspeed / l;
velocityDesired.East *= groundspeed / l;
}
float downError = altitudeSetpoint - positionState.Down;
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
// update pathstatus
pathStatus.error = progress.error;
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
VelocityDesiredSet(&velocityDesired);
@ -357,13 +343,14 @@ 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.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
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;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
@ -389,8 +376,7 @@ static uint8_t updateFixedDesiredAttitude()
AirspeedStateData airspeedState;
SystemSettingsData systemSettings;
float groundspeedState;
float groundspeedDesired;
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
@ -401,10 +387,9 @@ static uint8_t updateFixedDesiredAttitude()
float descentspeedError;
float powerCommand;
float bearing;
float heading;
float headingError;
float course;
float airspeedVector[2];
float fluidMovement[2];
float courseComponent[2];
float courseError;
float courseCommand;
@ -418,29 +403,59 @@ static uint8_t updateFixedDesiredAttitude()
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* Compute speed error (required for throttle and pitch)
* Compute speed error and course
*/
// missing sensors for airspeed-direction we have to assume within
// reasonable error that measured airspeed is actually the airspeed
// component in forward pointing direction
// airspeedVector is normalized
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
// Current ground speed
groundspeedState = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North);
// note that airspeedStateBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement,
// but thanks to accelerometers, groundspeed reacts faster to changes in direction
// current ground speed projected in forward direction
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedState = groundspeedState + indicatedAirspeedStateBias;
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
// Desired ground speed
groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedStateBias,
fixedwingpathfollowerSettings.HorizontalVelMin,
fixedwingpathfollowerSettings.HorizontalVelMax);
// fluidMovement is a vector describing the aproximate movement vector of
// the surrounding fluid in 2d space (aka wind vector)
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
// calculate the movement vector we need to fly to reach velocityDesired -
// taking fluidMovement into account
courseComponent[0] = velocityDesired.North - fluidMovement[0];
courseComponent[1] = velocityDesired.East - fluidMovement[1];
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
fixedwingpathfollowerSettings.HorizontalVelMin,
fixedwingpathfollowerSettings.HorizontalVelMax);
// if we could fly at arbitrary speeds, we'd just have to move towards the
// courseComponent vector as previously calculated and we'd be fine
// unfortunately however we are bound by min and max air speed limits, so
// we need to recalculate the correct course to meet at least the
// velocityDesired vector direction at our current speed
// this overwrites courseComponent
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
// Error condition: wind speed too high, we can't go where we want anymore
fixedwingpathfollowerStatus.Errors.Wind = 0;
if ((!valid) &&
fixedwingpathfollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.Wind = 1;
result = 0;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = bound(
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedwingpathfollowerSettings.VerticalVelMax,
fixedwingpathfollowerSettings.VerticalVelMax);
@ -466,32 +481,27 @@ static uint8_t updateFixedDesiredAttitude()
result = 0;
}
if (indicatedAirspeedState < 1e-6f) {
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
// also we cannot handle planes flying backwards, lets just wait until the nose drops
fixedwingpathfollowerStatus.Errors.Lowspeed = 1;
return 0;
/**
* Compute desired thrust command
*/
// compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0.0f) {
powerIntegral = boundf(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
} else {
powerIntegral = 0.0f;
}
/**
* Compute desired throttle command
*/
// compute saturated integral error throttle 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,
fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
} else { powerIntegral = 0; }
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = bound(
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp,
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max,
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
);
// Compute final throttle response
// Compute final thrust response
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp +
powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki +
speedErrorToPowerCommandComponent;
@ -501,27 +511,27 @@ 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 = boundf(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
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError > 0 && // we are too slow already
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError > 0.0f && // we are too slow already
fixedwingpathfollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
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
velocityState.Down < 0 && // we ARE going up
descentspeedDesired > 0 && // we WANT to go down
airspeedError < 0 && // we are too fast already
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0.0f && // we ARE going up
descentspeedDesired > 0.0f && // we WANT to go down
airspeedError < 0.0f && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.Highpower = 1;
result = 0;
@ -531,19 +541,18 @@ static uint8_t updateFixedDesiredAttitude()
/**
* Compute desired pitch command
*/
if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
// Integrate with saturation
airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
}
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
);
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp
@ -554,55 +563,25 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt;
fixedwingpathfollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit.Min,
fixedwingpathfollowerSettings.PitchLimit.Max);
stabDesired.Pitch = boundf(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit.Min,
fixedwingpathfollowerSettings.PitchLimit.Max);
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
if (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
if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError < 0.0f && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
}
/**
* Calculate where we are heading and why (wind issues)
*/
bearing = attitudeState.Yaw;
heading = RAD2DEG(atan2f(velocityState.East, velocityState.North));
headingError = heading - bearing;
if (headingError < -180.0f) {
headingError += 360.0f;
}
if (headingError > 180.0f) {
headingError -= 360.0f;
}
// Error condition: wind speed is higher than airspeed. We are forced backwards!
fixedwingpathfollowerStatus.Errors.Wind = 0;
if ((headingError > fixedwingpathfollowerSettings.Safetymargins.Wind ||
headingError < -fixedwingpathfollowerSettings.Safetymargins.Wind) &&
fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
// we are flying backwards
fixedwingpathfollowerStatus.Errors.Wind = 1;
result = 0;
}
/**
* Compute desired roll command
*/
if (groundspeedDesired > 1e-6f) {
course = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North));
courseError = course - heading;
} else {
// if we are not supposed to move, run in a circle
courseError = -90.0f;
result = 0;
}
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (courseError < -180.0f) {
courseError += 360.0f;
@ -611,9 +590,22 @@ static uint8_t updateFixedDesiredAttitude()
courseError -= 360.0f;
}
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
-fixedwingpathfollowerSettings.CoursePI.ILimit,
fixedwingpathfollowerSettings.CoursePI.ILimit);
// overlap calculation. Theres a dead zone behind the craft where the
// counter-yawing of some craft while rolling could render a desired right
// turn into a desired left turn. Making the turn direction based on
// current roll angle keeps the plane committed to a direction once chosen
if (courseError < -180.0f + (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll > 0.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f - (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll < 0.0f) {
courseError -= 360.0f;
}
courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
-fixedwingpathfollowerSettings.CoursePI.ILimit,
fixedwingpathfollowerSettings.CoursePI.ILimit);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp +
courseIntegral);
@ -621,10 +613,10 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral;
fixedwingpathfollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.Neutral +
courseCommand,
fixedwingpathfollowerSettings.RollLimit.Min,
fixedwingpathfollowerSettings.RollLimit.Max);
stabDesired.Roll = boundf(fixedwingpathfollowerSettings.RollLimit.Neutral +
courseCommand,
fixedwingpathfollowerSettings.RollLimit.Min,
fixedwingpathfollowerSettings.RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
@ -633,12 +625,13 @@ static uint8_t updateFixedDesiredAttitude()
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0;
stabDesired.Yaw = 0.0f;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
@ -647,20 +640,6 @@ static uint8_t updateFixedDesiredAttitude()
return result;
}
/**
* Bound input value between limits
*/
static float bound(float val, float min, float max)
{
if (val < min) {
val = min;
} else if (val > max) {
val = max;
}
return val;
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
@ -674,10 +653,116 @@ static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
AirspeedStateGet(&airspeedState);
VelocityStateGet(&velocityState);
float groundspeed = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North);
float airspeedVector[2];
float yaw;
AttitudeStateYawGet(&yaw);
airspeedVector[0] = cos_lookup_deg(yaw);
airspeedVector[1] = sin_lookup_deg(yaw);
// vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeed;
// note - we do fly by Indicated Airspeed (== calibrated airspeed)
// however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement.
indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
// note - we do fly by Indicated Airspeed (== calibrated airspeed) however
// since airspeed is updated less often than groundspeed, we use sudden
// changes to groundspeed to offset the airspeed by the same measurement.
// This has a side effect that in the absence of any airspeed updates, the
// pathfollower will fly using groundspeed.
}
/**
* Function to calculate course vector C based on airspeed s, fluid movement F
* and desired movement vector V
* parameters in: V,F,s
* parameters out: C
* returns true if a valid solution could be found for V,F,s, false if not
* C will be set to a best effort attempt either way
*/
static bool correctCourse(float *C, float *V, float *F, float s)
{
// Approach:
// Let Sc be a circle around origin marking possible movement vectors
// of the craft with airspeed s (all possible options for C)
// Let Vl be a line through the origin along movement vector V where fr any
// point v on line Vl v = k * (V / |V|) = k' * V
// Let Wl be a line parallel to Vl where for any point v on line Vl exists
// a point w on WL with w = v - F
// Then any intersection between circle Sc and line Wl represents course
// vector which would result in a movement vector
// V' = k * ( V / |V|) = k' * V
// If there is no intersection point, S is insufficient to compensate
// for F and we can only try to fly in direction of V (thus having wind drift
// but at least making progress orthogonal to wind)
s = fabsf(s);
float f = sqrtf(F[0] * F[0] + F[1] * F[1]);
// normalize Cn=V/|V|, |V| must be >0
float v = sqrtf(V[0] * V[0] + V[1] * V[1]);
if (v < 1e-6f) {
// if |V|=0, we aren't supposed to move, turn into the wind
// (this allows hovering)
C[0] = -F[0];
C[1] = -F[1];
// if desired airspeed matches fluidmovement a hover is actually
// intended so return true
return fabsf(f - s) < 1e-3f;
}
float Vn[2] = { V[0] / v, V[1] / v };
// project F on V
float fp = F[0] * Vn[0] + F[1] * Vn[1];
// find component Fo of F that is orthogonal to V
// (which is exactly the distance between Vl and Wl)
float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1];
// find k where k * Vn = C - Fo
// |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
// so k^2 + fo^2 = s^2 (since |Vn|=1)
float k2 = s * s - fo2;
if (k2 <= -1e-3f) {
// there is no solution, we will be drifted off either way
// fallback: fly stupidly in direction of V and hope for the best
C[0] = V[0];
C[1] = V[1];
return false;
} else if (k2 <= 1e-3f) {
// there is exactly one solution: -Fo
C[0] = -Fo[0];
C[1] = -Fo[1];
return true;
}
// we have two possible solutions k positive and k negative as there are
// two intersection points between Wl and Sc
// which one is better? two criteria:
// 1. we MUST move in the right direction, if any k leads to -v its invalid
// 2. we should minimize the speed error
float k = sqrt(k2);
float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
// project C+F on Vn to find signed resulting movement vector length
float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
// in this case the angle between course and resulting movement vector
// is greater than 90 degrees - so we actually fly backwards
C[0] = C1[0];
C[1] = C1[1];
return true;
}
C[0] = C2[0];
C[1] = C2[1];
if (vp2 >= 0.0f) {
// in this case the angle between course and movement vector is less than
// 90 degrees, but we do move in the right direction
return true;
} else {
// in this case we actually get driven in the opposite direction of V
// with both solutions for C
// this might be reached in headwind stronger than maximum allowed
// airspeed.
return false;
}
}

28
flight/modules/GPS/GPS.c
View File

@ -63,21 +63,24 @@ static float GravityAccel(float latitude, float longitude, float altitude);
// ****************
// Private constants
#define GPS_TIMEOUT_MS 500
#define GPS_TIMEOUT_MS 500
// delay from detecting HomeLocation.Set == False before setting new homelocation
// this prevent that a save with homelocation.Set = false triggered by gps ends saving
// the new location with Set = true.
#define GPS_HOMELOCATION_SET_DELAY 5000
#ifdef PIOS_GPS_SETS_HOMELOCATION
// Unfortunately need a good size stack for the WMM calculation
#define STACK_SIZE_BYTES 1024
#define STACK_SIZE_BYTES 1024
#else
#if defined(PIOS_GPS_MINIMAL)
#define STACK_SIZE_BYTES 500
#define STACK_SIZE_BYTES 500
#else
#define STACK_SIZE_BYTES 650
#define STACK_SIZE_BYTES 650
#endif // PIOS_GPS_MINIMAL
#endif // PIOS_GPS_SETS_HOMELOCATION
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
// ****************
// Private variables
@ -200,6 +203,9 @@ static void gpsTask(__attribute__((unused)) void *parameters)
portTickType xDelay = 100 / portTICK_RATE_MS;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
#ifdef PIOS_GPS_SETS_HOMELOCATION
portTickType homelocationSetDelay = 0;
#endif
GPSPositionSensorData gpspositionsensor;
GPSSettingsData gpsSettings;
@ -260,7 +266,15 @@ static void gpsTask(__attribute__((unused)) void *parameters)
HomeLocationGet(&home);
if (home.Set == HOMELOCATION_SET_FALSE) {
setHomeLocation(&gpspositionsensor);
if (homelocationSetDelay == 0) {
homelocationSetDelay = xTaskGetTickCount();
}
if (xTaskGetTickCount() - homelocationSetDelay > GPS_HOMELOCATION_SET_DELAY) {
setHomeLocation(&gpspositionsensor);
homelocationSetDelay = 0;
}
} else {
homelocationSetDelay = 0;
}
#endif
} else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&

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

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

@ -0,0 +1,329 @@
/**
******************************************************************************
* @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:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
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);
}
}
/**
* @}
* @}
*/

224
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,62 @@
#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 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);
/**
* @brief Handler to setup takeofflocation on arming. it is set up during Arming
* @input: NONE:
* @output: NONE
*/
void takeOffLocationHandler();
/**
* @brief Initialize TakeoffLocationHanlder
*/
void takeOffLocationHandlerInit();
/*
* These are assumptions we make in the flight code about the order of settings and their consistency between
@ -61,54 +93,132 @@ 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_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#define assumptions2 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#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_STABILIZATION3SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#define assumptions4 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#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_STABILIZATION5SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#define assumptions6 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_MANUAL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_VIRTUALBAR == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYRATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_RELAYATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_ALTITUDEHOLD == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_VERTICALVELOCITY == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY) && \
((int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_CRUISECONTROL == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL) && \
1 \
)
#define assumptions7 \
( \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION4SETTINGS_NUMELEM) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION5SETTINGS_NUMELEM) && \
((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM == (int)FLIGHTMODESETTINGS_STABILIZATION6SETTINGS_NUMELEM) && \
1 \
)
#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_STABILIZED4 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED4) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED5) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED6) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) && \
1 \
)
#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

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

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

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

@ -0,0 +1,113 @@
/**
******************************************************************************
* @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)
#include <plans.h>
// 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) {
plan_initialize();
}
uint8_t flightMode;
FlightStatusFlightModeGet(&flightMode);
if (newinit) {
// After not being in this mode for a while init at current height
switch (flightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
plan_setup_returnToBase();
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
plan_setup_positionHold();
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
plan_setup_land();
break;
default:
plan_setup_positionHold();
/* 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;
}
}
switch (flightMode) {
// special handling of autoland - behaves like positon hold but with slow altitude decrease
case FLIGHTSTATUS_FLIGHTMODE_LAND:
plan_run_land();
break;
default:
break;
}
}
#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)
*
*/
}
/**
* @}
* @}
*/

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

@ -0,0 +1,153 @@
/**
******************************************************************************
* @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;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
stab_settings = cast_struct_to_array(settings.Stabilization4Settings, settings.Stabilization4Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
stab_settings = cast_struct_to_array(settings.Stabilization5Settings, settings.Stabilization5Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
stab_settings = cast_struct_to_array(settings.Stabilization6Settings, settings.Stabilization6Settings.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_MANUAL) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
(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 * stabSettings.RollMax :
(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_MANUAL) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
(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 * stabSettings.PitchMax :
(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_MANUAL) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
(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 * stabSettings.YawMax :
(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;
stabilization.StabilizationMode.Thrust = stab_settings[3];
StabilizationDesiredSet(&stabilization);
}
/**
* @}
* @}
*/

125
flight/modules/ManualControl/takeofflocationhandler.c Normal file
View File

@ -0,0 +1,125 @@
/**
******************************************************************************
*
* @file takeofflocationhandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief handles TakeOffLocation
* --
* @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 <stdint.h>
#include <flightstatus.h>
#include <takeofflocation.h>
#include <positionstate.h>
// Private constants
// Private types
// Private variables
static bool locationSet;
// Private functions
static void SetTakeOffLocation();
void takeOffLocationHandlerInit()
{
TakeOffLocationInitialize();
// check whether there is a preset/valid takeoff location
uint8_t mode;
uint8_t status;
TakeOffLocationModeGet(&mode);
TakeOffLocationStatusGet(&status);
// preset with invalid location will actually behave like FirstTakeoff
if (mode == TAKEOFFLOCATION_MODE_PRESET && status == TAKEOFFLOCATION_STATUS_VALID) {
locationSet = true;
} else {
locationSet = false;
status = TAKEOFFLOCATION_STATUS_INVALID;
TakeOffLocationStatusSet(&status);
}
}
/**
* Handles TakeOffPosition location setup
* @param newinit
*/
void takeOffLocationHandler()
{
uint8_t armed;
uint8_t status;
FlightStatusArmedGet(&armed);
// Location already acquired/preset
if (armed == FLIGHTSTATUS_ARMED_ARMED && locationSet) {
return;
}
TakeOffLocationStatusGet(&status);
switch (armed) {
case FLIGHTSTATUS_ARMED_ARMING:
case FLIGHTSTATUS_ARMED_ARMED:
if (!locationSet || status != TAKEOFFLOCATION_STATUS_VALID) {
uint8_t mode;
TakeOffLocationModeGet(&mode);
if ((mode != TAKEOFFLOCATION_MODE_PRESET) || (status == TAKEOFFLOCATION_STATUS_INVALID)) {
SetTakeOffLocation();
} else {
locationSet = true;
}
}
break;
case FLIGHTSTATUS_ARMED_DISARMED:
// unset if location is to be acquired at each arming
if (locationSet) {
uint8_t mode;
TakeOffLocationModeGet(&mode);
if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) {
locationSet = false;
status = TAKEOFFLOCATION_STATUS_INVALID;
TakeOffLocationStatusSet(&status);
}
}
break;
}
}
/**
* Retrieve TakeOffLocation from current PositionStatus
*/
void SetTakeOffLocation()
{
TakeOffLocationData takeOffLocation;
TakeOffLocationGet(&takeOffLocation);
PositionStateData positionState;
PositionStateGet(&positionState);
takeOffLocation.North = positionState.North;
takeOffLocation.East = positionState.East;
takeOffLocation.Down = positionState.Down;
takeOffLocation.Status = TAKEOFFLOCATION_STATUS_VALID;
TakeOffLocationSet(&takeOffLocation);
locationSet = true;
}

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.

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

@ -31,6 +31,7 @@
#include "openpilot.h"
#include "callbackinfo.h"
#include "pathplan.h"
#include "flightstatus.h"
#include "airspeedstate.h"
@ -41,9 +42,10 @@
#include "velocitystate.h"
#include "waypoint.h"
#include "waypointactive.h"
#include "manualcontrolsettings.h"
#include "flightmodesettings.h"
#include <pios_struct_helper.h>
#include "paths.h"
#include "plans.h"
// Private constants
#define STACK_SIZE_BYTES 1024
@ -88,6 +90,7 @@ static bool pathplanner_active = false;
*/
int32_t PathPlannerStart()
{
plan_initialize();
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(commandUpdated);
WaypointActiveConnectCallback(commandUpdated);
@ -95,7 +98,7 @@ int32_t PathPlannerStart()
PathStatusConnectCallback(statusUpdated);
// Start main task callback
DelayedCallbackDispatch(pathPlannerHandle);
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
return 0;
}
@ -115,8 +118,8 @@ int32_t PathPlannerInitialize()
WaypointInitialize();
WaypointActiveInitialize();
pathPlannerHandle = DelayedCallbackCreate(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, STACK_SIZE_BYTES);
pathDesiredUpdaterHandle = DelayedCallbackCreate(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, STACK_SIZE_BYTES);
pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
return 0;
}
@ -128,7 +131,7 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
*/
static void pathPlannerTask()
{
DelayedCallbackSchedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
PIOS_CALLBACKSCHEDULER_Schedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
bool endCondition = false;
@ -138,7 +141,7 @@ static void pathPlannerTask()
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
pathplanner_active = false;
if (!validPathPlan) {
// unverified path plans are only a warning while we are not in pathplanner mode
@ -168,21 +171,7 @@ static void pathPlannerTask()
if (!failsafeRTHset) {
failsafeRTHset = 1;
// copy pasta: same calculation as in manualcontrol, set return to home coordinates
PositionStateData positionState;
PositionStateGet(&positionState);
ManualControlSettingsData settings;
ManualControlSettingsGet(&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);
plan_setup_positionHold();
}
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
@ -332,13 +321,13 @@ static uint8_t checkPathPlan()
// callback function when status changed, issue execution of state machine
void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(pathDesiredUpdaterHandle);
PIOS_CALLBACKSCHEDULER_Dispatch(pathDesiredUpdaterHandle);
}
// callback function when waypoints changed in any way, update pathDesired
void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(pathPlannerHandle);
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
}

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

@ -268,6 +268,7 @@ static void registerObject(UAVObjHandle obj)
.obj = obj,
.instId = UAVOBJ_ALL_INSTANCES,
.event = EV_UPDATED_PERIODIC,
.lowPriority = true,
};
// Get metadata
@ -289,34 +290,33 @@ static void updateRadioComBridgeStats()
RadioComBridgeStatsData radioComBridgeStats;
// Get telemetry stats
UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats);
UAVTalkResetStats(data->telemUAVTalkCon);
UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats, true);
// Get radio stats
UAVTalkGetStats(data->radioUAVTalkCon, &radioUAVTalkStats);
UAVTalkResetStats(data->radioUAVTalkCon);
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.TelemetryTxRetries += data->telemetryTxRetries;
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.RadioTxRetries += data->radioTxRetries;
radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes;
radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors;
radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes;
radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors;
radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors;
radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors;
radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes;
radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors;
radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors;
radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors;
// Update stats object data
RadioComBridgeStatsSet(&radioComBridgeStats);
@ -342,11 +342,11 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
updateRadioComBridgeStats();
}
// 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->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0;
if (success == -1) {
while (retries <= MAX_RETRIES && ret == -1) {
ret = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS);
if (ret == -1) {
++retries;
}
}
@ -376,11 +376,11 @@ 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, ev.instId, 0, RETRY_TIMEOUT_MS) == 0;
if (success == -1) {
while (retries <= MAX_RETRIES && ret == -1) {
ret = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS);
if (ret == -1) {
++retries;
}
}
@ -413,8 +413,8 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
}
} else if (PIOS_COM_TELEMETRY) {
// Send the data straight to the telemetry port.
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
// 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) {
@ -513,12 +513,12 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
if (bytes_to_process > 0) {
// Send the data over the radio link.
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
// 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) {
PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
ret = PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
}
}
} else {
@ -547,8 +547,8 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
}
#endif /* PIOS_INCLUDE_USB */
if (outputPort) {
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
// 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) {
@ -577,8 +577,8 @@ 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)) {
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
// 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) {

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

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

@ -55,12 +55,14 @@
#include <attitudestate.h>
#include <attitudesettings.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <flightstatus.h>
#include <taskinfo.h>
#include <CoordinateConversions.h>
#include <pios_board_info.h>
#include <pios_struct_helper.h>
// Private constants
#define STACK_SIZE_BYTES 1000
@ -78,15 +80,17 @@ 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 };
static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
// 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 +117,14 @@ int32_t SensorsInitialize(void)
MagSensorInitialize();
RevoCalibrationInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
rotate = 0;
RevoCalibrationConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -341,12 +347,23 @@ 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 +377,22 @@ 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];
@ -387,20 +414,16 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0],
(float)values[0] * mag_scale[1] - mag_bias[1],
-(float)values[2] * mag_scale[2] - mag_bias[2] };
if (rotate) {
float mag_out[3];
rot_mult(R, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
} else {
mag.x = mags[0];
mag.y = mags[1];
mag.z = mags[2];
}
float mags[3] = { (float)values[1] - mag_bias[0],
(float)values[0] - mag_bias[1],
-(float)values[2] - mag_bias[2] };
float mag_out[3];
rot_mult(mag_transform, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
@ -421,43 +444,38 @@ 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_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;
AttitudeSettingsGet(&attitudeSettings);
// Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.Roll == 0 && attitudeSettings.BoardRotation.Pitch == 0 &&
attitudeSettings.BoardRotation.Yaw == 0) {
if (fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f
&& fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f &&
fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) {
rotate = 0;
} else {
float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;
}
float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
matrix_mult_3x3f((float(*)[3])cast_struct_to_array(cal.mag_transform, cal.mag_transform.r0c0), R, mag_transform);
}
/**
* @}

206
flight/modules/Stabilization/altitudeloop.c Normal file
View File

@ -0,0 +1,206 @@
/**
******************************************************************************
*
* @file altitudeloop.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @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 <callbackinfo.h>
#include <pid.h>
#include <altitudeloop.h>
#include <CoordinateConversions.h>
#include <altitudeholdsettings.h>
#include <altitudeholdstatus.h>
#include <velocitystate.h>
#include <positionstate.h>
// Private constants
#ifdef REVOLUTION
#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 CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define STACK_SIZE_BYTES 512
// Private types
// Private variables
static DelayedCallbackInfo *altitudeHoldCBInfo;
static AltitudeHoldSettingsData altitudeHoldSettings;
static struct pid pid0, pid1;
static ThrustModeType thrustMode;
static PiOSDeltatimeConfig timeval;
static float thrustSetpoint = 0.0f;
static float thrustDemand = 0.0f;
static float startThrust = 0.5f;
// Private functions
static void altitudeHoldTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void VelocityStateUpdatedCb(UAVObjEvent *ev);
/**
* Setup mode and setpoint
*/
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
{
static bool newaltitude = true;
if (reinit) {
startThrust = setpoint;
pid_zero(&pid0);
pid_zero(&pid1);
newaltitude = true;
}
const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// this is the max speed in m/s at the extents of thrust
float thrustRate;
uint8_t thrustExp;
AltitudeHoldSettingsThrustExpGet(&thrustExp);
AltitudeHoldSettingsThrustRateGet(&thrustRate);
PositionStateData posState;
PositionStateGet(&posState);
if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) {
// Cut thrust if desired
thrustSetpoint = 0.0f;
thrustDemand = 0.0f;
thrustMode = DIRECT;
newaltitude = true;
} else if (mode == ALTITUDEVARIO && setpoint > 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
thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
thrustMode = ALTITUDEVARIO;
newaltitude = true;
} else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) {
thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate);
thrustMode = ALTITUDEVARIO;
newaltitude = true;
} else if (newaltitude == true) {
thrustSetpoint = posState.Down;
thrustMode = ALTITUDEHOLD;
newaltitude = false;
}
return thrustDemand;
}
/**
* Initialise the module, called on startup
*/
void stabilizationAltitudeloopInit()
{
AltitudeHoldSettingsInitialize();
AltitudeHoldStatusInitialize();
PositionStateInitialize();
VelocityStateInitialize();
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
// Create object queue
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
VelocityStateConnectCallback(&VelocityStateUpdatedCb);
// Start main task
SettingsUpdatedCb(NULL);
}
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void)
{
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
// do the actual control loop(s)
float positionStateDown;
PositionStateDownGet(&positionStateDown);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
switch (thrustMode) {
case ALTITUDEHOLD:
// altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, thrustSetpoint, positionStateDown, dT);
break;
case ALTITUDEVARIO:
altitudeHoldStatus.VelocityDesired = thrustSetpoint;
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
break;
}
AltitudeHoldStatusSet(&altitudeHoldStatus);
switch (thrustMode) {
case DIRECT:
thrustDemand = thrustSetpoint;
break;
default:
// velocity control loop
thrustDemand = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
break;
}
}
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);
}
static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
}
#endif /* ifdef REVOLUTION */

293
flight/modules/Stabilization/cruisecontrol.c Normal file
View File

@ -0,0 +1,293 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief cruisecontrol mode
* @note This file implements the logic for a cruisecontrol
* @{
*
* @file cruisecontrol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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
*/
#include <openpilot.h>
#include <stabilization.h>
#include <attitudestate.h>
#include <sin_lookup.h>
static float cruisecontrol_factor = 1.0f;
static inline float CruiseControlLimitThrust(float thrust)
{
// limit to user specified absolute max thrust
return boundf(thrust, stabSettings.cruiseControl.min_thrust, stabSettings.cruiseControl.max_thrust);
}
// assumes 1.0 <= factor <= 100.0
// a factor of less than 1.0 could make it return a value less than stabSettings.cruiseControl.min_thrust
// CP helis need to have min_thrust=-1
//
// multicopters need to have min_thrust=0.05 or so
// values below that will not be subject to max / min limiting
// that means thrust can be less than min
// that means multicopter motors stop spinning at low stick
static inline float CruiseControlFactorToThrust(float factor, float thrust)
{
// don't touch thrust if it's less than min_thrust
// without that test, quadcopter props will spin up
// to min thrust even at zero throttle stick
// if Cruise Control is enabled on this flight switch position
if (thrust > stabSettings.cruiseControl.min_thrust) {
return CruiseControlLimitThrust(thrust * factor);
}
return thrust;
}
static float CruiseControlAngleToFactor(float angle)
{
float factor;
// avoid singularity
if (angle > 89.999f && angle < 90.001f) {
factor = stabSettings.settings.CruiseControlMaxPowerFactor;
} else {
// the simple bank angle boost calculation that Cruise Control revolves around
factor = 1.0f / fabsf(cos_lookup_deg(angle));
// factor in the power trim, no effect at 1.0, linear effect increases with factor
factor = (factor - 1.0f) * stabSettings.cruiseControl.power_trim + 1.0f;
// limit to user specified max power multiplier
if (factor > stabSettings.settings.CruiseControlMaxPowerFactor) {
factor = stabSettings.settings.CruiseControlMaxPowerFactor;
}
}
return factor;
}
void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand)
{
static float previous_angle;
static uint32_t previous_time = 0;
static bool previous_time_valid = false;
// For multiple, speedy flips this mainly strives to address the
// fact that (due to thrust delay) thrust didn't average straight
// down, but at an angle. For less speedy flips it acts like it
// used to. It can be turned off by setting power delay to 0.
// It takes significant time for the motors of a multi-copter to
// spin up. It takes significant time for the collective servo of
// a CP heli to move from one end to the other. Both of those are
// modeled here as linear, i.e. twice as much change takes twice
// as long. Given a correctly configured maximum delay time this
// code calculates how far in advance to start the control
// transition so that half way through the physical transition it
// is just crossing the transition angle.
// Example: Rotation rate = 360. Full stroke delay = 0.2
// Transition angle 90 degrees. Start the transition 0.1 second
// before 90 degrees (36 degrees at 360 deg/sec) and it will be
// complete 0.1 seconds after 90 degrees.
// Note that this code only handles the transition to/from inverted
// thrust. It doesn't handle the case where thrust is changed a
// lot in a small angle range when that range is close to 90 degrees.
// It doesn't handle the small constant "system delay" caused by the
// delay between reading sensors and actuators beginning to respond.
// It also assumes that the pilot is holding the throttle constant;
// when the pilot does change the throttle, the compensation is
// simply recalculated.
// This implementation of future thrust isn't perfect. That would
// probably require an iterative procedure for solving a
// transcendental equation of the form linear(x) = 1/cos(x). It's
// shortcomings generally don't hurt anything and work better than
// without it. It is designed to work perfectly if the pilot is
// using full thrust during flips and it is only activated if 70% or
// greater thrust is used.
uint32_t time = PIOS_DELAY_GetuS();
// Get roll and pitch angles, calculate combined angle, and begin
// the general algorithm.
// Example: 45 degrees roll plus 45 degrees pitch = 60 degrees
// Do it every 8th iteration to save CPU.
if (time != previous_time || previous_time_valid == false) {
float angle, angle_unmodified;
// spherical right triangle
// 0.0 <= angle <= 180.0
angle_unmodified = angle = RAD2DEG(acosf(cos_lookup_deg(attitude->Roll)
* cos_lookup_deg(attitude->Pitch)));
// Calculate rate as a combined (roll and pitch) bank angle
// change; in degrees per second. Rate is calculated over the
// most recent 8 loops through stabilization. We could have
// asked the gyros. This is probably cheaper.
if (previous_time_valid) {
float rate;
// rate can be negative.
rate = (angle - previous_angle) / ((float)(time - previous_time) / 1000000.0f);
// Define "within range" to be those transitions that should
// be executing now. Recall that each impulse transition is
// spread out over a range of time / angle.
// There is only one transition and the high power level for
// it is either:
// 1/fabsf(cos(angle)) * current thrust
// or max power factor * current thrust
// or full thrust
// You can cross the transition with angle either increasing
// or decreasing (rate positive or negative).
// Thrust is never boosted for negative values of
// thrustDemand (negative stick values)
//
// When the aircraft is upright, thrust is always boosted
// . for positive values of thrustDemand
// When the aircraft is inverted, thrust is sometimes
// . boosted or reversed (or combinations thereof) or zeroed
// . for positive values of thrustDemand
// It depends on the inverted power settings.
// Of course, you can set MaxPowerFactor to 1.0 to
// . effectively disable boost.
if (thrustDemand > 0.0f) {
// to enable the future thrust calculations, make sure
// there is a large enough transition that the result
// will be roughly on vs. off; without that, it can
// exaggerate the length of time the inverted to upright
// transition holds full throttle and reduce the length
// of time for full throttle when going upright to inverted.
if (thrustDemand > 0.7f) {
float thrust;
thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand);
// determine if we are in range of the transition
// given the thrust at max_angle and thrustDemand
// (typically close to 1.0), change variable 'thrust' to
// be the proportion of the largest thrust change possible
// that occurs when going into inverted mode.
// Example: 'thrust' is 0.8 A quad has min_thrust set
// to 0.05 The difference is 0.75. The largest possible
// difference with this setup is 0.9 - 0.05 = 0.85, so
// the proportion is 0.75/0.85
// That is nearly a full throttle stroke.
// the 'thrust' variable is non-negative here
switch (stabSettings.settings.CruiseControlInvertedPowerOutput) {
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
// normal multi-copter case, stroke is max to zero
// technically max to constant min_thrust
// can be used by CP
thrust = (thrust - CruiseControlLimitThrust(0.0f)) / stabSettings.cruiseControl.thrust_difference;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
// reversed but not boosted
// : CP heli case, stroke is max to -stick
// : thrust = (thrust - CruiseControlLimitThrust(-thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
// else it is both unreversed and unboosted
// : simply turn off boost, stroke is max to +stick
// : thrust = (thrust - CruiseControlLimitThrust(thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
thrust = (thrust - CruiseControlLimitThrust(
(stabSettings.settings.CruiseControlInvertedThrustReversing
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED)
? -thrustDemand
: thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
// if boosted and reversed
if (stabSettings.settings.CruiseControlInvertedThrustReversing
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
// CP heli case, stroke is max to min
thrust = (thrust - CruiseControlFactorToThrust(-CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
}
// else it is boosted and unreversed so the throttle doesn't change
else {
// CP heli case, no transition, so stroke is zero
thrust = 0.0f;
}
break;
}
// 'thrust' is now the proportion of max stroke
// multiply this proportion of max stroke,
// times the max stroke time, to get this stroke time
// we only want half of this time before the transition
// (and half after the transition)
thrust *= stabSettings.cruiseControl.half_power_delay;
// 'thrust' is now the length of time for this stroke
// multiply that times angular rate to get the lead angle
thrust *= fabsf(rate);
// if the transition is within range we use it,
// else we just use the current calculated thrust
if ((float)stabSettings.settings.CruiseControlMaxAngle - thrust <= angle
&& angle <= (float)stabSettings.settings.CruiseControlMaxAngle + thrust) {
// default to a little above max angle
angle = (float)stabSettings.settings.CruiseControlMaxAngle + 0.01f;
// if roll direction is downward
// then thrust value is taken from below max angle
// by the code that knows about the transition angle
if (rate < 0.0f) {
angle -= 0.02f;
}
}
} // if thrust > 0.7; else just use the angle we already calculated
cruisecontrol_factor = CruiseControlAngleToFactor(angle);
} else { // if thrust > 0 set factor from angle; else
cruisecontrol_factor = 1.0f;
}
if (angle >= (float)stabSettings.settings.CruiseControlMaxAngle) {
switch (stabSettings.settings.CruiseControlInvertedPowerOutput) {
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
cruisecontrol_factor = 0.0f;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
cruisecontrol_factor = 1.0f;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
// no change, leave factor >= 1.0 alone
break;
}
if (stabSettings.settings.CruiseControlInvertedThrustReversing
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
cruisecontrol_factor = -cruisecontrol_factor;
}
}
} // if previous_time_valid i.e. we've got a rate; else leave (angle and) factor alone
previous_time = time;
previous_time_valid = true;
previous_angle = angle_unmodified;
} // every 8th time
}
float cruisecontrol_apply_factor(float raw)
{
if (stabSettings.settings.CruiseControlMaxPowerFactor > 0.0001f) {
raw = CruiseControlFactorToThrust(cruisecontrol_factor, raw);
}
return raw;
}

41
flight/modules/Stabilization/inc/altitudeloop.h Normal file
View File

@ -0,0 +1,41 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief altitudeloop mode
* @note This file implements the logic for a altitudeloop
* @{
*
* @file altitudeloop.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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
*/
#ifndef ALTITUDELOOP_H
#define ALTITUDELOOP_H
typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType;
void stabilizationAltitudeloopInit();
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit);
#endif /* ALTITUDELOOP_H */

42
flight/modules/Stabilization/inc/cruisecontrol.h Normal file
View File

@ -0,0 +1,42 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief cruisecontrol mode
* @note This file implements the logic for a cruisecontrol
* @{
*
* @file cruisecontrol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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
*/
#ifndef CRUISECONTROL_H
#define CRUISECONTROL_H
#include <openpilot.h>
#include <attitudestate.h>
void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand);
float cruisecontrol_apply_factor(float raw);
#endif /* CRUISECONTROL_H */

38
flight/modules/Stabilization/inc/innerloop.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief innerloop mode
* @note This file implements the logic for a innerloop
* @{
*
* @file innerloop.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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
*/
#ifndef INNERLOOP_H
#define INNERLOOP_H
void stabilizationInnerloopInit();
#endif /* INNERLOOP_H */

38
flight/modules/Stabilization/inc/outerloop.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief outerloop mode
* @note This file implements the logic for a outerloop
* @{
*
* @file outerloop.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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
*/
#ifndef OUTERLOOP_H
#define OUTERLOOP_H
void stabilizationOuterloopInit();
#endif /* OUTERLOOP_H */

44
flight/modules/Stabilization/inc/stabilization.h
View File

@ -33,10 +33,52 @@
#ifndef STABILIZATION_H
#define STABILIZATION_H
enum { ROLL, PITCH, YAW, MAX_AXES };
#include <openpilot.h>
#include <pid.h>
#include <stabilizationsettings.h>
#include <stabilizationbank.h>
int32_t StabilizationInitialize();
typedef struct {
StabilizationSettingsData settings;
StabilizationBankData stabBank;
float gyro_alpha;
struct {
float min_thrust;
float max_thrust;
float thrust_difference;
float power_trim;
float half_power_delay;
float max_power_factor_angle;
} cruiseControl;
struct {
int8_t gyroupdates;
int8_t rateupdates;
} monitor;
float rattitude_mode_transition_stick_position;
struct pid innerPids[3], outerPids[3];
} StabilizationData;
extern StabilizationData stabSettings;
#define AXES 4
#define FAILSAFE_TIMEOUT_MS 30
#ifndef PIOS_STABILIZATION_STACK_SIZE
#define STACK_SIZE_BYTES 800
#else
#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE
#endif
// must be same as eventdispatcher to avoid needing additional mutexes
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
// outer loop only executes every 4th uavobject update to safe CPU
#define OUTERLOOP_SKIPCOUNT 4
#endif // STABILIZATION_H
/**

289
flight/modules/Stabilization/innerloop.c Normal file
View File

@ -0,0 +1,289 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Stabilization PID loops in an airframe type independent manner
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
* @{
*
* @file innerloop.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pid.h>
#include <callbackinfo.h>
#include <ratedesired.h>
#include <actuatordesired.h>
#include <gyrostate.h>
#include <airspeedstate.h>
#include <stabilizationstatus.h>
#include <flightstatus.h>
#include <manualcontrolcommand.h>
#include <stabilizationbank.h>
#include <stabilization.h>
#include <relay_tuning.h>
#include <virtualflybar.h>
#include <cruisecontrol.h>
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
#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 variables
static DelayedCallbackInfo *callbackHandle;
static float gyro_filtered[3] = { 0, 0, 0 };
static float axis_lock_accum[3] = { 0, 0, 0 };
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static PiOSDeltatimeConfig timeval;
static float speedScaleFactor = 1.0f;
// Private functions
static void stabilizationInnerloopTask();
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
#ifdef REVOLUTION
static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
#endif
void stabilizationInnerloopInit()
{
RateDesiredInitialize();
ActuatorDesiredInitialize();
GyroStateInitialize();
StabilizationStatusInitialize();
FlightStatusInitialize();
ManualControlCommandInitialize();
#ifdef REVOLUTION
AirspeedStateInitialize();
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
#endif
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES);
GyroStateConnectCallback(GyroStateUpdatedCb);
// schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
/**
* WARNING! This callback executes with critical flight control priority every
* time a gyroscope update happens do NOT put any time consuming calculations
* in this loop unless they really have to execute with every gyro update
*/
static void stabilizationInnerloopTask()
{
// watchdog and error handling
{
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif
bool warn = false;
bool error = false;
bool crit = false;
// check if outer loop keeps executing
if (stabSettings.monitor.rateupdates > -64) {
stabSettings.monitor.rateupdates--;
}
if (stabSettings.monitor.rateupdates < -(2 * OUTERLOOP_SKIPCOUNT)) {
// warning if rate loop skipped more than 2 execution
warn = true;
}
if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) {
// error if rate loop skipped more than 4 executions
error = true;
}
// check if gyro keeps updating
if (stabSettings.monitor.gyroupdates < 1) {
// critical if gyro didn't update at all!
crit = true;
}
if (stabSettings.monitor.gyroupdates > 1) {
// warning if we missed a gyro update
warn = true;
}
if (stabSettings.monitor.gyroupdates > 3) {
// error if we missed 3 gyro updates
error = true;
}
stabSettings.monitor.gyroupdates = 0;
if (crit) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL);
} else if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
} else if (warn) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
RateDesiredData rateDesired;
ActuatorDesiredData actuator;
StabilizationStatusInnerLoopData enabled;
FlightStatusControlChainData cchain;
RateDesiredGet(&rateDesired);
ActuatorDesiredGet(&actuator);
StabilizationStatusInnerLoopGet(&enabled);
FlightStatusControlChainGet(&cchain);
float *rate = &rateDesired.Roll;
float *actuatorDesiredAxis = &actuator.Roll;
int t;
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
if (reinit) {
stabSettings.innerPids[t].iAccumulator = 0;
}
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings);
break;
case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING:
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit);
break;
case STABILIZATIONSTATUS_INNERLOOP_AXISLOCK:
if (fabsf(rate[t]) > stabSettings.settings.MaxAxisLockRate) {
// While getting strong commands act like rate mode
axis_lock_accum[t] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[t] += (rate[t] - gyro_filtered[t]) * dT;
axis_lock_accum[t] = boundf(axis_lock_accum[t], -stabSettings.settings.MaxAxisLock, stabSettings.settings.MaxAxisLock);
rate[t] = axis_lock_accum[t] * stabSettings.settings.AxisLockKp;
}
// IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication!
// keep order as it is, RATE must follow!
case STABILIZATIONSTATUS_INNERLOOP_RATE:
// limit rate to maximum configured limits (once here instead of 5 times in outer loop)
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
} else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL:
actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
}
actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
}
actuator.UpdateTime = dT * 1000;
if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuator);
} else {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
{
uint8_t armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);
if (armed != FLIGHTSTATUS_ARMED_ARMED ||
((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
}
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
GyroStateData gyroState;
GyroStateGet(&gyroState);
gyro_filtered[0] = gyro_filtered[0] * stabSettings.gyro_alpha + gyroState.x * (1 - stabSettings.gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * stabSettings.gyro_alpha + gyroState.y * (1 - stabSettings.gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * stabSettings.gyro_alpha + gyroState.z * (1 - stabSettings.gyro_alpha);
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
stabSettings.monitor.gyroupdates++;
}
#ifdef REVOLUTION
static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
AirspeedStateData airspeedState;
AirspeedStateGet(&airspeedState);
if (stabSettings.settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
// feature has been turned off
speedScaleFactor = 1.0f;
} else {
// scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
speedScaleFactor = boundf((stabSettings.settings.ScaleToAirspeed * stabSettings.settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed),
stabSettings.settings.ScaleToAirspeedLimits.Min,
stabSettings.settings.ScaleToAirspeedLimits.Max);
}
}
#endif
/**
* @}
* @}
*/

298
flight/modules/Stabilization/outerloop.c Normal file
View File

@ -0,0 +1,298 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Stabilization PID loops in an airframe type independent manner
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
* @{
*
* @file outerloop.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization 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
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pid.h>
#include <callbackinfo.h>
#include <ratedesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <stabilizationstatus.h>
#include <flightstatus.h>
#include <manualcontrolcommand.h>
#include <stabilizationbank.h>
#include <stabilization.h>
#include <cruisecontrol.h>
#include <altitudeloop.h>
#include <CoordinateConversions.h>
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#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 variables
static DelayedCallbackInfo *callbackHandle;
static AttitudeStateData attitude;
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static PiOSDeltatimeConfig timeval;
// Private functions
static void stabilizationOuterloopTask();
static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
void stabilizationOuterloopInit()
{
RateDesiredInitialize();
StabilizationDesiredInitialize();
AttitudeStateInitialize();
StabilizationStatusInitialize();
FlightStatusInitialize();
ManualControlCommandInitialize();
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES);
AttitudeStateConnectCallback(AttitudeStateUpdatedCb);
}
/**
* WARNING! This callback executes with critical flight control priority every
* time a gyroscope update happens do NOT put any time consuming calculations
* in this loop unless they really have to execute with every gyro update
*/
static void stabilizationOuterloopTask()
{
AttitudeStateData attitudeState;
RateDesiredData rateDesired;
StabilizationDesiredData stabilizationDesired;
StabilizationStatusOuterLoopData enabled;
AttitudeStateGet(&attitudeState);
StabilizationDesiredGet(&stabilizationDesired);
RateDesiredGet(&rateDesired);
StabilizationStatusOuterLoopGet(&enabled);
float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
int t;
float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
float local_error[3];
{
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4];
float q_error[4];
for (t = 0; t < 3; t++) {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
rpy_desired[t] = stabilizationDesiredAxis[t];
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rpy_desired[t] = ((float *)&attitudeState.Roll)[t];
break;
}
}
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, &attitudeState.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else /* if defined(PIOS_QUATERNION_STABILIZATION) */
// Simpler algorithm for CC, less memory
local_error[0] = stabilizationDesiredAxis[0] - attitudeState.Roll;
local_error[1] = stabilizationDesiredAxis[1] - attitudeState.Pitch;
local_error[2] = stabilizationDesiredAxis[2] - attitudeState.Yaw;
// find shortest way
float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
if (modulo < 0) {
local_error[2] = modulo + 180.0f;
} else {
local_error[2] = modulo - 180.0f;
}
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
}
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
if (reinit) {
stabSettings.outerPids[t].iAccumulator = 0;
}
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
break;
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
{
float stickinput[3];
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t];
// limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
-cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t]
);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
// - this makes the stick region into a square;
// - this is a feature!
// - hold a roll angle and add just pitch without the stick sensitivity changing
float magnitude = fabsf(stickinput[t]);
if (t < 2) {
magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
}
// 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 <= stabSettings.rattitude_mode_transition_stick_position) {
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
} else {
magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
* (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
/ (1.0f - stabSettings.rattitude_mode_transition_stick_position)
+ STICK_VALUE_AT_MODE_TRANSITION;
}
rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
// FIXME: local_error[] is rate - attitude for Weak Leveling
// The only ramifications are:
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
// Changing Rate mode max rate currently requires a change to Kp
// That would be changed to Attitude mode max angle affecting Kp
// Also does not take dT into account
{
float rate_input = cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t] * stabilizationDesiredAxis[t] / cast_struct_to_array(stabSettings.stabBank, stabSettings.stabBank.RollMax)[t];
float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[t] = rate_input + weak_leveling;
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
} else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
#ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
break;
case STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
break;
#endif /* REVOLUTION */
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
}
}
RateDesiredSet(&rateDesired);
{
uint8_t armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);
if (armed != FLIGHTSTATUS_ARMED_ARMED ||
((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
}
// update cruisecontrol based on attitude
cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
stabSettings.monitor.rateupdates = 0;
}
static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// to reduce CPU utilisation, outer loop is not executed every state update
static uint8_t cpusafer = 0;
if ((cpusafer++ % OUTERLOOP_SKIPCOUNT) == 0) {
// this does not need mutex protection as both eventdispatcher and stabi run in same callback task!
AttitudeStateGet(&attitude);
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
}
}
/**
* @}
* @}
*/

2
flight/modules/Stabilization/relay_tuning.c
View File

@ -63,7 +63,7 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
portTickType thisTime = xTaskGetTickCount();
static bool rateRelayRunning[MAX_AXES];
static bool rateRelayRunning[3];
// This indicates the current estimate of the smoothed error. So when it is high
// we are waiting for it to go low.

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

@ -33,129 +33,56 @@
#include <openpilot.h>
#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"
#include "relaytuningsettings.h"
#include "stabilizationdesired.h"
#include "attitudestate.h"
#include "airspeedstate.h"
#include "gyrostate.h"
#include "flightstatus.h"
#include "manualcontrolsettings.h"
#include "manualcontrol.h" // Just to get a macro
#include "taskinfo.h"
#include <pid.h>
#include <manualcontrolcommand.h>
#include <flightmodesettings.h>
#include <stabilizationsettings.h>
#include <stabilizationdesired.h>
#include <stabilizationstatus.h>
#include <stabilizationbank.h>
#include <stabilizationsettingsbank1.h>
#include <stabilizationsettingsbank2.h>
#include <stabilizationsettingsbank3.h>
#include <relaytuning.h>
#include <relaytuningsettings.h>
#include <ratedesired.h>
#include <sin_lookup.h>
#include <stabilization.h>
#include <innerloop.h>
#include <outerloop.h>
#include <altitudeloop.h>
// Math libraries
#include "CoordinateConversions.h"
#include "pid.h"
#include "sin_lookup.h"
// Includes for various stabilization algorithms
#include "relay_tuning.h"
#include "virtualflybar.h"
// Includes for various stabilization algorithms
#include "relay_tuning.h"
// Private constants
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
#define MAX_QUEUE_SIZE 1
#if defined(PIOS_STABILIZATION_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE
#else
#define STACK_SIZE_BYTES 840
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority
#define FAILSAFE_TIMEOUT_MS 30
// number of flight mode switch positions
#define NUM_FMS_POSITIONS 6
// 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 { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET };
enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET };
// Public variables
StabilizationData stabSettings;
// Private variables
static xTaskHandle taskHandle;
static StabilizationSettingsData settings;
static xQueueHandle queue;
float gyro_alpha = 0;
float axis_lock_accum[3] = { 0, 0, 0 };
uint8_t max_axis_lock = 0;
uint8_t max_axislock_rate = 0;
float weak_leveling_kp = 0;
uint8_t weak_leveling_max = 0;
bool lowThrottleZeroIntegral;
bool lowThrottleZeroAxis[MAX_AXES];
float vbar_decay = 0.991f;
struct pid pids[PID_MAX];
int cur_flight_mode = -1;
static uint8_t rattitude_anti_windup;
static float cruise_control_min_throttle;
static float cruise_control_max_throttle;
static 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[NUM_FMS_POSITIONS];
static int cur_flight_mode = -1;
// Private functions
static void stabilizationTask(void *parameters);
static float bound(float val, float range);
static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void BankUpdatedCb(UAVObjEvent *ev);
static void SettingsBankUpdatedCb(UAVObjEvent *ev);
static float stab_log2f(float x);
static float stab_powf(float x, uint8_t p);
static void FlightModeSwitchUpdatedCb(UAVObjEvent *ev);
static void StabilizationDesiredUpdatedCb(UAVObjEvent *ev);
/**
* Module initialization
*/
int32_t StabilizationStart()
{
// Initialize variables
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
// AttitudeStateConnectQueue(queue);
GyroStateConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
StabilizationBankConnectCallback(BankUpdatedCb);
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
BankUpdatedCb(StabilizationBankHandle());
// Start main task
xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
#endif
@ -167,752 +94,190 @@ int32_t StabilizationStart()
*/
int32_t StabilizationInitialize()
{
// stop the compile if the number of switch positions changes, but has not been changed here
PIOS_STATIC_ASSERT(NUM_FMS_POSITIONS == sizeof(((ManualControlSettingsData *)0)->FlightModePosition) / sizeof((((ManualControlSettingsData *)0)->FlightModePosition)[0]));
// Initialize variables
StabilizationDesiredInitialize();
StabilizationSettingsInitialize();
StabilizationStatusInitialize();
StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
StabilizationSettingsBank2Initialize();
StabilizationSettingsBank3Initialize();
ActuatorDesiredInitialize();
#ifdef DIAG_RATEDESIRED
RateDesiredInitialize();
#endif
#ifdef REVOLUTION
AirspeedStateInitialize();
#endif
ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch
// Code required for relay tuning
sin_lookup_initalize();
RelayTuningSettingsInitialize();
RelayTuningInitialize();
stabilizationOuterloopInit();
stabilizationInnerloopInit();
#ifdef REVOLUTION
stabilizationAltitudeloopInit();
#endif
pid_zero(&stabSettings.outerPids[0]);
pid_zero(&stabSettings.outerPids[1]);
pid_zero(&stabSettings.outerPids[2]);
pid_zero(&stabSettings.innerPids[0]);
pid_zero(&stabSettings.innerPids[1]);
pid_zero(&stabSettings.innerPids[2]);
return 0;
}
MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
/**
* Module task
*/
static void stabilizationTask(__attribute__((unused)) void *parameters)
static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
UAVObjEvent ev;
PiOSDeltatimeConfig timeval;
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
RateDesiredData rateDesired;
AttitudeStateData attitudeState;
GyroStateData gyroStateData;
FlightStatusData flightStatus;
StabilizationBankData stabBank;
#ifdef REVOLUTION
AirspeedStateData airspeedState;
#endif
SettingsUpdatedCb((UAVObjEvent *)NULL);
// Main task loop
ZeroPids();
while (1) {
float dT;
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif
// Wait until the Gyro object is updated, if a timeout then go to failsafe
if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
continue;
}
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
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
AirspeedStateGet(&airspeedState);
if (settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
// feature has been turned off
speedScaleFactor = 1.0f;
} else {
// scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed);
if (speedScaleFactor < settings.ScaleToAirspeedLimits.Min) {
speedScaleFactor = settings.ScaleToAirspeedLimits.Min;
}
if (speedScaleFactor > settings.ScaleToAirspeedLimits.Max) {
speedScaleFactor = settings.ScaleToAirspeedLimits.Max;
}
}
#else
const float speedScaleFactor = 1.0f;
#endif
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4];
float q_error[4];
float local_error[3];
// Essentially zero errors for anything in rate or none
if (stabDesired.StabilizationMode.Roll == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[0] = stabDesired.Roll;
} else {
rpy_desired[0] = attitudeState.Roll;
}
if (stabDesired.StabilizationMode.Pitch == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[1] = stabDesired.Pitch;
} else {
rpy_desired[1] = attitudeState.Pitch;
}
if (stabDesired.StabilizationMode.Yaw == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[2] = stabDesired.Yaw;
} else {
rpy_desired[2] = attitudeState.Yaw;
}
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, &attitudeState.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else /* if defined(PIOS_QUATERNION_STABILIZATION) */
// Simpler algorithm for CC, less memory
float local_error[3] = { stabDesired.Roll - attitudeState.Roll,
stabDesired.Pitch - attitudeState.Pitch,
stabDesired.Yaw - attitudeState.Yaw };
// find shortest way
float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
if (modulo < 0) {
local_error[2] = modulo + 180.0f;
} else {
local_error[2] = modulo - 180.0f;
}
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
float gyro_filtered[3];
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyroStateData.x * (1 - gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyroStateData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyroStateData.z * (1 - gyro_alpha);
float *stabDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
ActuatorDesiredGet(&actuatorDesired);
// A flag to track which stabilization mode each axis is in
static uint8_t previous_mode[MAX_AXES] = { 255, 255, 255 };
bool error = false;
// Run the selected stabilization algorithm on each axis:
for (uint8_t i = 0; i < MAX_AXES; i++) {
// Check whether this axis mode needs to be reinitialized
bool reinit = (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i] != previous_mode[i]);
previous_mode[i] = cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i];
// Apply the selected control law
switch (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i]) {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[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);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
if (reinit) {
pids[PID_ROLL + i].iAccumulator = 0;
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
// 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(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);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
// A parameterization from Attitude mode at center stick
// - to Rate mode at full stick
// This is done by parameterizing to use the rotation rate that Attitude mode
// - would use at center stick to using the rotation rate that Rate mode
// - would use at full stick in a weighted average sort of way.
{
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(stabBank.ManualRate, stabBank.ManualRate.Roll)[i];
// Compute what Attitude mode would give for this stick angle's rate
// stabDesired for this mode is [-1.0f,+1.0f]
// - multiply by Attitude mode max angle to get desired angle
// - 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(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(stabBank.MaximumRate,
stabBank.MaximumRate.Roll)[i]);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
// - this makes the stick region into a square;
// - this is a feature!
// - hold a roll angle and add just pitch without the stick sensitivity changing
// magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch);
float magnitude;
magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch));
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);
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];
// Run a virtual flybar stabilization algorithm on this axis
stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
// FIXME: local_error[] is rate - attitude for Weak Leveling
// The only ramifications are:
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
// Changing Rate mode max rate currently requires a change to Kp
// That would be changed to Attitude mode max angle affecting Kp
// Also does not take dT into account
{
if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
float weak_leveling = local_error[i] * weak_leveling_kp;
weak_leveling = bound(weak_leveling, weak_leveling_max);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode
rateDesiredAxis[i] = stabDesiredAxis[i];
axis_lock_accum[i] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT;
axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock);
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
}
rateDesiredAxis[i] = bound(rateDesiredAxis[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);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(stabDesiredAxis[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);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
if (reinit) {
pids[PID_ROLL + i].iAccumulator = 0;
}
// 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(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);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
actuatorDesiredAxis[i] = bound(stabDesiredAxis[i], 1.0f);
break;
default:
error = true;
break;
}
}
if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) {
stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);
}
#ifdef DIAG_RATEDESIRED
RateDesiredSet(&rateDesired);
#endif
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Throttle = stabDesired.Throttle;
// Suppress desired output while disarmed or throttle low, for configured axis
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || stabDesired.Throttle < 0) {
if (lowThrottleZeroAxis[ROLL]) {
actuatorDesired.Roll = 0.0f;
}
if (lowThrottleZeroAxis[PITCH]) {
actuatorDesired.Pitch = 0.0f;
}
if (lowThrottleZeroAxis[YAW]) {
actuatorDesired.Yaw = 0.0f;
}
}
// modify throttle according to 1/cos(bank angle)
// to maintain same altitdue with changing bank angle
// but without manually adjusting throttle
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
if (flight_mode_switch_position < NUM_FMS_POSITIONS
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
&& 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 throttle is getting reversed
// we may as well do pitch and yaw for a complete "invert switch"
actuatorDesired.Pitch = -actuatorDesired.Pitch;
actuatorDesired.Yaw = -actuatorDesired.Yaw;
}
}
}
// also don't adjust throttle if <= 0, leaves neg alone and zero throttle stops motors
if (actuatorDesired.Throttle > cruise_control_min_throttle) {
// quad example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8
// CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7
actuatorDesired.Throttle = (actuatorDesired.Throttle - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
if (actuatorDesired.Throttle > cruise_control_max_throttle) {
actuatorDesired.Throttle = cruise_control_max_throttle;
} else if (actuatorDesired.Throttle < cruise_control_min_throttle) {
actuatorDesired.Throttle = cruise_control_min_throttle;
}
}
}
if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
ActuatorDesiredSet(&actuatorDesired);
} else {
// Force all axes to reinitialize when engaged
for (uint8_t i = 0; i < MAX_AXES; i++) {
previous_mode[i] = 255;
}
}
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0)) {
// Force all axes to reinitialize when engaged
for (uint8_t i = 0; i < MAX_AXES; i++) {
previous_mode[i] = 255;
}
}
// Clear or set alarms. Done like this to prevent toggline each cycle
// and hammering system alarms
if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
StabilizationStatusData status;
StabilizationDesiredStabilizationModeData mode;
int t;
StabilizationDesiredStabilizationModeGet(&mode);
for (t = 0; t < AXES; t++) {
switch (cast_struct_to_array(mode, mode.Roll)[t]) {
case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
}
}
StabilizationStatusSet(&status);
}
/**
* Clear the accumulators and derivatives for all the axes
*/
static void ZeroPids(void)
static void FlightModeSwitchUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
for (uint32_t i = 0; i < PID_MAX; i++) {
pid_zero(&pids[i]);
}
for (uint8_t i = 0; i < 3; i++) {
axis_lock_accum[i] = 0.0f;
uint8_t fm;
ManualControlCommandFlightModeSwitchPositionGet(&fm);
if (fm == cur_flight_mode) {
return;
}
cur_flight_mode = fm;
SettingsBankUpdatedCb(NULL);
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if (val < -range) {
return -range;
} else if (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)
{
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;
}
return retval;
}
static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
if (cur_flight_mode < 0 || cur_flight_mode >= NUM_FMS_POSITIONS) {
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)) {
if ((ev) && ((stabSettings.settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
(stabSettings.settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
(stabSettings.settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
stabSettings.settings.FlightModeMap[cur_flight_mode] > 2)) {
return;
}
StabilizationBankData bank;
switch (settings.FlightModeMap[cur_flight_mode]) {
switch (stabSettings.settings.FlightModeMap[cur_flight_mode]) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&stabSettings.stabBank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&stabSettings.stabBank);
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&stabSettings.stabBank);
break;
}
StabilizationBankSet(&bank);
StabilizationBankSet(&stabSettings.stabBank);
}
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
}
*/
StabilizationBankGet(&stabSettings.stabBank);
// Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], bank.RollRatePID.Kp,
bank.RollRatePID.Ki,
bank.RollRatePID.Kd,
bank.RollRatePID.ILimit);
pid_configure(&stabSettings.innerPids[0], stabSettings.stabBank.RollRatePID.Kp,
stabSettings.stabBank.RollRatePID.Ki,
stabSettings.stabBank.RollRatePID.Kd,
stabSettings.stabBank.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);
pid_configure(&stabSettings.innerPids[1], stabSettings.stabBank.PitchRatePID.Kp,
stabSettings.stabBank.PitchRatePID.Ki,
stabSettings.stabBank.PitchRatePID.Kd,
stabSettings.stabBank.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);
pid_configure(&stabSettings.innerPids[2], stabSettings.stabBank.YawRatePID.Kp,
stabSettings.stabBank.YawRatePID.Ki,
stabSettings.stabBank.YawRatePID.Kd,
stabSettings.stabBank.YawRatePID.ILimit);
// Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], bank.RollPI.Kp,
bank.RollPI.Ki,
pid_configure(&stabSettings.outerPids[0], stabSettings.stabBank.RollPI.Kp,
stabSettings.stabBank.RollPI.Ki,
0,
bank.RollPI.ILimit);
stabSettings.stabBank.RollPI.ILimit);
// Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], bank.PitchPI.Kp,
bank.PitchPI.Ki,
pid_configure(&stabSettings.outerPids[1], stabSettings.stabBank.PitchPI.Kp,
stabSettings.stabBank.PitchPI.Ki,
0,
bank.PitchPI.ILimit);
stabSettings.stabBank.PitchPI.ILimit);
// Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], bank.YawPI.Kp,
bank.YawPI.Ki,
pid_configure(&stabSettings.outerPids[2], stabSettings.stabBank.YawPI.Kp,
stabSettings.stabBank.YawPI.Ki,
0,
bank.YawPI.ILimit);
// Set the Rattitude roll rate PID constants
pid_configure(&pids[PID_RATEA_ROLL],
bank.RollRatePID.Kp,
bank.RollRatePID.Ki,
bank.RollRatePID.Kd,
bank.RollRatePID.ILimit);
// Set the Rattitude pitch rate PID constants
pid_configure(&pids[PID_RATEA_PITCH],
bank.PitchRatePID.Kp,
bank.PitchRatePID.Ki,
bank.PitchRatePID.Kd,
bank.PitchRatePID.ILimit);
// Set the Rattitude yaw rate PID constants
pid_configure(&pids[PID_RATEA_YAW],
bank.YawRatePID.Kp,
bank.YawRatePID.Ki,
bank.YawRatePID.Kd,
bank.YawRatePID.ILimit);
stabSettings.stabBank.YawPI.ILimit);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationSettingsGet(&settings);
// needs no mutex, as long as eventdispatcher and Stabilization are both TASK_PRIORITY_CRITICAL
StabilizationSettingsGet(&stabSettings.settings);
// Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
// Maximum deviation to accumulate for axis lock
max_axis_lock = settings.MaxAxisLock;
max_axislock_rate = settings.MaxAxisLockRate;
// Settings for weak leveling
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;
pid_configure_derivative(stabSettings.settings.DerivativeCutoff, stabSettings.settings.DerivativeGamma);
// 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
@ -920,33 +285,29 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
// calculation
const float fakeDt = 0.0025f;
if (settings.GyroTau < 0.0001f) {
gyro_alpha = 0; // not trusting this to resolve to 0
if (stabSettings.settings.GyroTau < 0.0001f) {
stabSettings.gyro_alpha = 0; // not trusting this to resolve to 0
} else {
gyro_alpha = expf(-fakeDt / settings.GyroTau);
stabSettings.gyro_alpha = expf(-fakeDt / stabSettings.settings.GyroTau);
}
// Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau);
// force flight mode update
cur_flight_mode = -1;
// Rattitude flight mode anti-windup factor
rattitude_anti_windup = settings.RattitudeAntiWindup;
// Rattitude stick angle where the attitude to rate transition happens
if (stabSettings.settings.RattitudeModeTransition < (uint8_t)10) {
stabSettings.rattitude_mode_transition_stick_position = 10.0f / 100.0f;
} else {
stabSettings.rattitude_mode_transition_stick_position = (float)stabSettings.settings.RattitudeModeTransition / 100.0f;
}
cruise_control_min_throttle = (float)settings.CruiseControlMinThrottle / 100.0f;
cruise_control_max_throttle = (float)settings.CruiseControlMaxThrottle / 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;
stabSettings.cruiseControl.min_thrust = (float)stabSettings.settings.CruiseControlMinThrust / 100.0f;
stabSettings.cruiseControl.max_thrust = (float)stabSettings.settings.CruiseControlMaxThrust / 100.0f;
stabSettings.cruiseControl.thrust_difference = stabSettings.cruiseControl.max_thrust - stabSettings.cruiseControl.min_thrust;
memcpy(
cruise_control_flight_mode_switch_pos_enable,
settings.CruiseControlFlightModeSwitchPosEnable,
sizeof(cruise_control_flight_mode_switch_pos_enable));
stabSettings.cruiseControl.power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f;
stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f;
stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor));
}
/**

26
flight/modules/Stabilization/virtualflybar.c
View File

@ -37,12 +37,9 @@
#include "stabilizationsettings.h"
// ! Private variables
static float vbar_integral[MAX_AXES];
static float vbar_integral[3];
static float vbar_decay = 0.991f;
// ! Private methods
static float bound(float val, float range);
int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings)
{
float gyro_gain = 1.0f;
@ -54,7 +51,7 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
// Track the angle of the virtual flybar which includes a slow decay
vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT;
vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle);
vbar_integral[axis] = boundf(vbar_integral[axis], -settings->VbarMaxAngle, settings->VbarMaxAngle);
// Command signal can indicate how much to disregard the gyro feedback (fast flips)
if (settings->VbarGyroSuppress > 0) {
@ -64,15 +61,15 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
// Get the settings for the correct axis
switch (axis) {
case ROLL:
case 0:
kp = settings->VbarRollPI.Kp;
ki = settings->VbarRollPI.Ki;
break;
case PITCH:
case 1:
kp = settings->VbarPitchPI.Kp;
ki = settings->VbarPitchPI.Ki;;
break;
case YAW:
case 2:
kp = settings->VbarYawPI.Kp;
ki = settings->VbarYawPI.Ki;
break;
@ -105,16 +102,3 @@ int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT)
return 0;
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if (val < -range) {
val = -range;
} else if (val > range) {
val = range;
}
return val;
}

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

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

@ -33,21 +33,29 @@
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <altitudefiltersettings.h>
#include <homelocation.h>
#include <CoordinateConversions.h>
// Private constants
#define STACK_REQUIRED 128
// duration of accel bias initialization phase
#define INITIALIZATION_DURATION_MS 5000
#define DT_ALPHA 1e-2f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_AVERAGE 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 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
@ -56,7 +64,9 @@ struct data {
float accelLast;
float baroLast;
bool first_run;
portTickType initTimer;
AltitudeFilterSettingsData settings;
float gravity;
};
// Private variables
@ -65,6 +75,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,8 +83,11 @@ int32_t filterAltitudeInitialize(stateFilter *handle)
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
HomeLocationInitialize();
AttitudeStateInitialize();
AltitudeFilterSettingsInitialize();
AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb);
reloadSettings = true;
return STACK_REQUIRED;
}
@ -81,10 +95,10 @@ 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;
@ -96,7 +110,7 @@ static int32_t init(stateFilter *self)
this->baroLast = 0.0f;
this->accelLast = 0.0f;
this->first_run = 1;
AltitudeFilterSettingsGet(&this->settings);
HomeLocationg_eGet(&this->gravity);
return 0;
}
@ -104,10 +118,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_baro)) {
this->first_run = 0;
this->initTimer = xTaskGetTickCount();
}
} else {
// save existing position and velocity updates so GPS will still work
@ -115,13 +135,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
@ -129,54 +149,57 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
AttitudeStateGet(&att);
float Rbe[3][3];
Quaternion2R(&att.q1, Rbe);
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f);
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + this->gravity);
// 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_DELTATIME_GetAverageSeconds(&this->dt1config);
float speedLast = this->state[1];
float speedLast = this->velocityState;
this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * dT;
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]) * dT;
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_DELTATIME_GetAverageSeconds(&this->dt2config);
this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
this->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;
}
}
@ -184,6 +207,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
return 0;
}
void settingsUpdatedCb(UAVObjEvent *ev)
{
if (ev->obj == AltitudeFilterSettingsHandle()) {
reloadSettings = true;
}
}
/**
* @}

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

@ -36,7 +36,7 @@
// Private constants
#define STACK_REQUIRED 64
#define STACK_REQUIRED 128
#define INIT_CYCLES 100
// Private types
@ -44,30 +44,59 @@ 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->gpsAlt = 0.0f;
this->first_run = INIT_CYCLES;
RevoSettingsInitialize();
@ -81,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 = ((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)*this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * state->baro[0];
this->baroAlt = 0;
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]);
}
@ -100,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;
}

4
flight/modules/StateEstimation/filtercf.c
View File

@ -42,7 +42,7 @@
#include <revocalibration.h>
#include <CoordinateConversions.h>
#include <pios_notify.h>
// Private constants
#define STACK_REQUIRED 512
@ -295,6 +295,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
this->accel_filter_enabled = false;
this->rollPitchBiasRate = 0.01f;
this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
} else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
this->attitudeSettings.AccelKp = 1.0f;
this->attitudeSettings.AccelKi = 0.0f;
@ -303,6 +304,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
this->rollPitchBiasRate = 0.01f;
this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
this->init = 0;
PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
} else if (this->init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&this->attitudeSettings);

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

@ -36,6 +36,7 @@
#include <ekfconfiguration.h>
#include <ekfstatevariance.h>
#include <attitudestate.h>
#include <systemalarms.h>
#include <homelocation.h>
#include <insgps.h>
@ -348,7 +349,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
INSCovariancePrediction(dT);
if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS;
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.Magnetometer == SYSTEMALARMS_ALARM_OK) {
sensors |= MAG_SENSORS;
}
}
if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {
@ -356,6 +361,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
}
INSSetMagNorth(this->homeLocation.Be);
INSSetG(this->homeLocation.g_e);
if (!this->usePos) {
// position and velocity variance used in indoor mode

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

@ -33,6 +33,8 @@
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <revocalibration.h>
#include <revosettings.h>
#include <systemalarms.h>
#include <homelocation.h>
#include <CoordinateConversions.h>
@ -43,9 +45,13 @@
// Private types
struct data {
HomeLocationData homeLocation;
RevoCalibrationData revoCalibration;
float magBias[3];
RevoSettingsData revoSettings;
uint8_t warningcount;
uint8_t errorcount;
float homeLocationBe[3];
float magBe2;
float magBias[3];
};
// Private variables
@ -54,6 +60,7 @@ struct data {
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static void checkMagValidity(struct data *this, float mag[3]);
static void magOffsetEstimation(struct data *this, float mag[3]);
@ -70,9 +77,13 @@ static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
HomeLocationGet(&this->homeLocation);
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
this->warningcount = this->errorcount = 0;
HomeLocationBeGet(this->homeLocationBe);
// magBe2 holds the squared magnetic vector length (extpected)
this->magBe2 = this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2];
RevoCalibrationGet(&this->revoCalibration);
RevoSettingsGet(&this->revoSettings);
return 0;
}
@ -81,6 +92,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
struct data *this = (struct data *)self->localdata;
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
checkMagValidity(this, state->mag);
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
}
@ -89,6 +101,53 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
return 0;
}
/**
* check validity of magnetometers
*/
static void checkMagValidity(struct data *this, float mag[3])
{
#define ALARM_THRESHOLD 5
// mag2 holds the actual magnetic vector length (squared)
float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2];
// warning and error thresholds
// avoud sqrt() : minlimit<actual<maxlimit === minlimit²<actual²<maxlimit²
//
// actual = |mag|
// minlimit = |Be| - maxDeviation*|Be| = |Be| * (1 - maxDeviation)
// maxlimit = |Be| + maxDeviation*|Be| = |Be| * (1 + maxDeviation)
// minlimit² = |Be|² * ( 1 - 2*maxDeviation + maxDeviation²)
// maxlimit² = |Be|² * ( 1 + 2*maxDeviation + maxDeviation²)
//
float minWarning2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
float maxWarning2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
float minError2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
float maxError2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
// set errors
if (minWarning2 < mag2 && mag2 < maxWarning2) {
this->warningcount = 0;
this->errorcount = 0;
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
} else if (minError2 < mag2 && mag2 < maxError2) {
this->errorcount = 0;
if (this->warningcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
} else {
this->warningcount++;
}
} else {
if (this->errorcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
} else {
this->errorcount++;
}
}
}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
@ -137,8 +196,8 @@ static void magOffsetEstimation(struct data *this, float mag[3])
}
#else // if 0
const float Rxy = sqrtf(this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1]);
const float Rz = this->homeLocation.Be[2];
const float Rxy = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1]);
const float Rz = this->homeLocationBe[2];
const float rate = this->revoCalibration.MagBiasNullingRate;
float Rot[3][3];

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

148
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;
@ -132,21 +155,35 @@ static stateFilter cfmFilter;
static stateFilter ekf13iFilter;
static stateFilter ekf13Filter;
// this is a hack to provide a computational shortcut for faster gyro state progression
static float gyroRaw[3];
static float gyroDelta[3];
// preconfigured filter chains selectable via revoSettings.FusionAlgorithm
static filterPipeline *cfQueue = &(filterPipeline) {
static const filterPipeline *cfQueue = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfFilter,
.next = NULL,
}
}
}
};
static const filterPipeline *cfmiQueue = &(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 = &cfmFilter,
.next = NULL,
}
}
}
@ -176,15 +213,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 +245,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 +271,8 @@ int32_t StateEstimationInitialize(void)
GPSVelocitySensorInitialize();
GPSPositionSensorInitialize();
HomeLocationInitialize();
GyroStateInitialize();
AccelStateInitialize();
MagStateInitialize();
@ -245,6 +282,8 @@ int32_t StateEstimationInitialize(void)
RevoSettingsConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&homeLocationUpdatedCb);
GyroSensorConnectCallback(&sensorUpdatedCb);
AccelSensorConnectCallback(&sensorUpdatedCb);
MagSensorConnectCallback(&sensorUpdatedCb);
@ -256,6 +295,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 +306,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 +337,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 +345,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 +367,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 +404,7 @@ static void StateEstimationCb(void)
return;
} else {
// set new fusion algortithm
filterChain = (filterPipeline *)newFilterChain;
filterChain = newFilterChain;
fusionAlgorithm = revoSettings.FusionAlgorithm;
}
}
@ -373,22 +416,27 @@ static void StateEstimationCb(void)
// fetch sensors, check values, and load into state struct
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
gyroRaw[0] = states.gyro[0];
gyroRaw[1] = states.gyro[1];
gyroRaw[2] = states.gyro[2];
}
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
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,13 +453,18 @@ static void StateEstimationCb(void)
if (!current) {
runState = RUNSTATE_SAVE;
}
DelayedCallbackDispatch(stateEstimationCallback);
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
break;
case RUNSTATE_SAVE:
// the final output of filters is saved in state variables
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z);
// EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
gyroDelta[0] = states.gyro[0] - gyroRaw[0];
gyroDelta[1] = states.gyro[1] - gyroRaw[1];
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
@ -458,9 +511,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 +528,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
@ -487,6 +551,14 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
if (ev->obj == GyroSensorHandle()) {
updatedSensors |= SENSORUPDATES_gyro;
// shortcut - update GyroState right away
GyroSensorData s;
GyroStateData t;
GyroSensorGet(&s);
t.x = s.x + gyroDelta[0];
t.y = s.y + gyroDelta[1];
t.z = s.z + gyroDelta[2];
GyroStateSet(&t);
}
if (ev->obj == AccelSensorHandle()) {
@ -513,7 +585,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
updatedSensors |= SENSORUPDATES_airspeed;
}
DelayedCallbackDispatch(stateEstimationCallback);
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
}

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

@ -42,7 +42,7 @@
#include <pios_struct_helper.h>
// private includes
#include "inc/systemmod.h"
#include "notification.h"
// UAVOs
#include <objectpersistence.h>
@ -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)
@ -72,39 +72,32 @@
#endif
// Private constants
#define SYSTEM_UPDATE_PERIOD_MS 1000
#define LED_BLINK_RATE_HZ 5
#ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c
// must be updated if the FreeRTOS or compiler
// optimisation options are changed.
#endif
#define SYSTEM_UPDATE_PERIOD_MS 250
#if defined(PIOS_SYSTEM_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE
#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE
#else
#define STACK_SIZE_BYTES 1024
#define STACK_SIZE_BYTES 1024
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
// 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;
// Private functions
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 +117,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 +141,7 @@ int32_t SystemModInitialize(void)
ObjectPersistenceInitialize();
#ifdef DIAG_TASKS
TaskInfoInitialize();
CallbackInfoInitialize();
#endif
#ifdef DIAG_I2C_WDG_STATS
I2CStatsInitialize();
@ -169,13 +164,11 @@ MODULE_INITCALL(SystemModInitialize, 0);
*/
static void systemTask(__attribute__((unused)) void *parameters)
{
uint8_t cycleCount = 0;
/* create all modules thread */
MODULE_TASKCREATE_ALL;
/* start the delayed callback scheduler */
CallbackSchedulerStart();
PIOS_CALLBACKSCHEDULER_Start();
if (mallocFailed) {
/* We failed to malloc during task creation,
@ -184,16 +177,10 @@ 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 +191,13 @@ static void systemTask(__attribute__((unused)) void *parameters)
#ifdef DIAG_TASKS
TaskInfoData taskInfoData;
CallbackInfoData callbackInfoData;
#endif
// Main system loop
while (1) {
NotificationUpdateStatus();
// Update the system statistics
updateStats();
cycleCount = cycleCount > 0 ? cycleCount - 1 : 7;
// Update the system alarms
updateSystemAlarms();
#ifdef DIAG_I2C_WDG_STATS
@ -223,37 +209,17 @@ 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;
FlightStatusArmedGet(&armingStatus);
if ((armingStatus == FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x1)) ||
(armingStatus != FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x4))) {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
#endif /* PIOS_LED_HEARTBEAT */
// Turn on the error LED if an alarm is set
#if defined(PIOS_LED_ALARM)
if (AlarmsHasCritical()) {
PIOS_LED_On(PIOS_LED_ALARM);
} else if ((AlarmsHasErrors() && (cycleCount & 0x1)) ||
(!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x4))) {
PIOS_LED_On(PIOS_LED_ALARM);
} else {
PIOS_LED_Off(PIOS_LED_ALARM);
}
#endif /* PIOS_LED_ALARM */
// }
UAVObjEvent ev;
int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2);
int delayTime = SYSTEM_UPDATE_PERIOD_MS;
#if defined(PIOS_INCLUDE_RFM22B)
@ -440,7 +406,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,7 +501,6 @@ static uint16_t GetFreeIrqStackSize(void)
*/
static void updateStats()
{
static portTickType lastTickCount = 0;
SystemStatsData stats;
// Get stats and update
@ -534,10 +518,6 @@ static void updateStats()
// 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);
@ -550,16 +530,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);
}
@ -602,10 +577,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
@ -614,7 +594,7 @@ static void updateSystemAlarms()
UAVObjClearStats();
EventClearStats();
if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) {
AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_ERROR);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
}
@ -630,19 +610,12 @@ static void updateSystemAlarms()
}
/**
* Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
* Called by the RTOS when the CPU is idle,
*/
void vApplicationIdleHook(void)
{
// Called when the scheduler has no tasks to run
if (idleCounterClear == 0) {
++idleCounter;
} else {
idleCounter = 0;
idleCounterClear = 0;
}
NotificationOnboardLedsRun();
}
/**
* Called by the RTOS when a stack overflow is detected.
*/
@ -650,7 +623,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) {

164
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,13 +158,16 @@ 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
@ -276,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);
}
}
/**
@ -374,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.
@ -435,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]);
@ -451,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 */
/**
@ -487,10 +522,13 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_UPDATED_PERIODIC;
ev.lowPriority = true;
ret = EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
if (ret == -1) {
ret = EventPeriodicQueueCreate(&ev, queue, updatePeriodMs);
ret = EventPeriodicQueueCreate(&ev, targetQueue, updatePeriodMs);
}
return ret;
}
@ -511,10 +549,13 @@ static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_LOGGING_PERIODIC;
ev.lowPriority = true;
ret = EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
if (ret == -1) {
ret = EventPeriodicQueueCreate(&ev, queue, updatePeriodMs);
ret = EventPeriodicQueueCreate(&ev, targetQueue, updatePeriodMs);
}
return ret;
}
@ -549,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);
@ -622,11 +661,10 @@ static void updateTelemetryStats()
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
}
// Update the telemetry alarm
// TODO: check whether is there any error condition worth raising an alarm
// Disconnection is actually a normal (non)working status so it is not raising alarms anymore.
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
}
// Update object
@ -683,17 +721,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;

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

@ -111,6 +111,7 @@ int32_t TxPIDInitialize(void)
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
@ -326,7 +327,7 @@ static void updatePIDs(UAVObjEvent *ev)
break;
case 2:
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
break;
default:

259
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"
@ -101,7 +101,6 @@ static void updatePathVelocity();
static void updateEndpointVelocity();
static void updateFixedAttitude(float *attitude);
static void updateVtolDesiredAttitude(bool yaw_attitude);
static float bound(float val, float min, float max);
static bool vtolpathfollower_enabled;
static void accessoryUpdated(UAVObjEvent *ev);
@ -140,6 +139,7 @@ int32_t VtolPathFollowerInitialize()
AccessoryDesiredInitialize();
PoiLearnSettingsInitialize();
PoiLocationInitialize();
HomeLocationInitialize();
vtolpathfollower_enabled = true;
} else {
vtolpathfollower_enabled = false;
@ -158,7 +158,8 @@ static float northPosIntegral = 0;
static float eastPosIntegral = 0;
static float downPosIntegral = 0;
static float throttleOffset = 0;
static float thrustOffset = 0;
static float gravity;
/**
* Module thread, should not return.
*/
@ -182,6 +183,7 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
HomeLocationg_eGet(&gravity);
SystemSettingsGet(&systemSettings);
if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
@ -206,55 +208,53 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
PathDesiredGet(&pathDesired);
// Check the combinations of flightmode and pathdesired mode
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
} else {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch (pathDesired.Mode) {
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
break;
case FLIGHTSTATUS_FLIGHTMODE_POI:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
} else {
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
@ -263,12 +263,10 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
eastPosIntegral = 0;
downPosIntegral = 0;
// Track throttle before engaging this mode. Cheap system ident
// Track thrust before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break;
thrustOffset = stabDesired.Thrust;
}
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
@ -398,7 +396,7 @@ static void updatePathVelocity()
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1);
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) {
groundspeed = 0;
}
@ -407,7 +405,7 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1);
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) {
groundspeed = 0;
}
@ -431,14 +429,14 @@ static void updatePathVelocity()
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * bound(progress.fractional_progress, 0, 1);
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1);
float downError = altitudeSetpoint - positionState.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus
pathStatus.error = progress.error;
@ -474,51 +472,17 @@ 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;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
northError = pathDesired.End.North - positionState.North;
northPosIntegral = boundf(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral);
eastError = pathDesired.End.East - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
eastError = pathDesired.End.East - positionState.East;
eastPosIntegral = boundf(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral);
// Limit the maximum velocity
@ -531,12 +495,12 @@ void updateEndpointVelocity()
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
downError = pathDesired.End.Down - downPos;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downError = pathDesired.End.Down - positionState.Down;
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
}
@ -550,13 +514,14 @@ 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.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
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;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
@ -599,12 +564,12 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
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);
@ -613,7 +578,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
downVel = gpsVelocity.Down;
}
break;
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_GROUNDSPEED:
{
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
@ -633,18 +598,18 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
// Compute desired north command
northError = velocityDesired.North - northVel;
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
northVelIntegral = boundf(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd
+ velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired east command
eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
eastVelIntegral = boundf(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
@ -653,28 +618,28 @@ 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 = boundf(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 = boundf(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
stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
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;
@ -685,6 +650,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw;
}
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
@ -720,7 +686,7 @@ static void updateNedAccel()
accel_ned[i] += Rbe[j][i] * accel[j];
}
}
accel_ned[2] += 9.81f;
accel_ned[2] += gravity;
NedAccelData accelData;
NedAccelGet(&accelData);
@ -730,19 +696,6 @@ static void updateNedAccel()
NedAccelSet(&accelData);
}
/**
* Bound input value between limits
*/
static float bound(float val, float min, float max)
{
if (val < min) {
val = min;
} else if (val > max) {
val = max;
}
return val;
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);

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

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

@ -346,6 +346,7 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u
* \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)
@ -358,7 +359,7 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
}
PIOS_Assert(com_dev->has_tx);
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->sendbuffer_sem, 0) != pdTRUE) {
if (xSemaphoreTake(com_dev->sendbuffer_sem, 5) != pdTRUE) {
return -2;
}
#endif /* PIOS_INCLUDE_FREERTOS */

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

49
flight/pios/common/pios_notify.c Normal file
View File

@ -0,0 +1,49 @@
/**
******************************************************************************
*
* @file pios_notify.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Handles user notifications.
* --
* @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_notify.h"
static volatile pios_notify_notification currentNotification = NOTIFY_NONE;
static volatile pios_notify_priority currentPriority;
void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority)
{
if (currentNotification == NOTIFY_NONE || currentPriority < priority) {
currentPriority = priority;
currentNotification = notification;
}
}
pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear)
{
pios_notify_notification ret = currentNotification;
if (clear && ret != NOTIFY_NONE) {
currentNotification = NOTIFY_NONE;
}
return ret;
}

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

@ -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);

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

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

58
flight/pios/inc/pios_notify.h Normal file
View File

@ -0,0 +1,58 @@
/**
******************************************************************************
*
* @file pios_notify.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Handles user notifications.
* --
* @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_NOTIFY_H_
#define PIOS_NOTIFY_H_
#include <stdbool.h>
typedef enum {
NOTIFY_NONE = 0,
NOTIFY_OK,
NOTIFY_NOK,
NOTIFY_DRAW_ATTENTION
} pios_notify_notification;
typedef enum {
NOTIFY_PRIORITY_CRITICAL = 2,
NOTIFY_PRIORITY_REGULAR = 1,
NOTIFY_PRIORITY_LOW = 0,
} pios_notify_priority;
/**
* start a new notification. If a notification is active it will be overwritten if its priority is lower than the new one.
* The new will be discarded otherwise
* @param notification kind of notification
* @param priority priority of the new notification
*/
void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority);
/**
* retrieve any active notification
* @param clear
* @return
*/
pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear);
#endif /* PIOS_NOTIFY_H_ */

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

147
flight/pios/inc/pios_ws2811.h Normal file
View File

@ -0,0 +1,147 @@
/**
******************************************************************************
*
* @file pios_ws2811.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief A driver for ws2811 rgb led controller.
* this is a plain PiOS port of the very clever solution
* implemented by Omri Iluz in the chibios driver here:
* https://github.com/omriiluz/WS2812B-LED-Driver-ChibiOS
* @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_WS2811_H_
#define PIOS_WS2811_H_
#include <stdint.h>
#include <pios_gpio.h>
#include <pios_stm32.h>
#include <pios_gpio_priv.h>
#include <stm32f4xx.h>
#include <stm32f4xx_tim.h>
#include <stm32f4xx_dma.h>
#define sign(x) ((x > 0) - (x < 0))
#define PIOS_WS2811_NUMLEDS 2
#define PIOS_WS2811_BUFFER_SIZE (((PIOS_WS2811_NUMLEDS) * 24))
#define PIOS_WS2811_MEMORYDATASIZE DMA_MemoryDataSize_HalfWord
#define PIOS_WS2811_PERIPHERALDATASIZE DMA_PeripheralDataSize_HalfWord
#define PIOS_WS2811_TIM_PERIOD 20
// Following times are keept on the lower side to accounts
// for bus contentions and irq response time
#define PIOS_WS2811_T0_HIGH_PERIOD 25 // .35us +/- 150nS
#define PIOS_WS2811_T1_HIGH_PERIOD 60 // .70us +/- 150nS
#define PIOS_WS2811_DMA_CH1_CONFIG(channel) \
{ \
.DMA_BufferSize = PIOS_WS2811_BUFFER_SIZE, \
.DMA_Channel = channel, \
.DMA_DIR = DMA_DIR_MemoryToPeripheral, \
.DMA_FIFOMode = DMA_FIFOMode_Enable, \
.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull, \
.DMA_Memory0BaseAddr = 0, \
.DMA_MemoryBurst = DMA_MemoryBurst_INC4, \
.DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \
.DMA_MemoryInc = DMA_MemoryInc_Enable, \
.DMA_Mode = DMA_Mode_Circular, \
.DMA_PeripheralBaseAddr = 0, \
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \
.DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \
.DMA_PeripheralInc = DMA_PeripheralInc_Disable, \
.DMA_Priority = DMA_Priority_VeryHigh, }
#define PIOS_WS2811_DMA_CH2_CONFIG(channel) \
{ \
.DMA_BufferSize = 4, \
.DMA_Channel = channel, \
.DMA_DIR = DMA_DIR_MemoryToPeripheral, \
.DMA_FIFOMode = DMA_FIFOMode_Enable, \
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \
.DMA_Memory0BaseAddr = 0, \
.DMA_MemoryBurst = DMA_MemoryBurst_INC4, \
.DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \
.DMA_MemoryInc = DMA_MemoryInc_Enable, \
.DMA_Mode = DMA_Mode_Circular, \
.DMA_PeripheralBaseAddr = 0, \
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \
.DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \
.DMA_PeripheralInc = DMA_PeripheralInc_Disable, \
.DMA_Priority = DMA_Priority_VeryHigh, }
#define PIOS_WS2811_DMA_UPDATE_CONFIG(channel) \
{ \
.DMA_BufferSize = 4, \
.DMA_Channel = channel, \
.DMA_DIR = DMA_DIR_MemoryToPeripheral, \
.DMA_FIFOMode = DMA_FIFOMode_Enable, \
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \
.DMA_Memory0BaseAddr = 0, \
.DMA_MemoryBurst = DMA_MemoryBurst_INC4, \
.DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \
.DMA_MemoryInc = DMA_MemoryInc_Enable, \
.DMA_Mode = DMA_Mode_Circular, \
.DMA_PeripheralBaseAddr = 0, \
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \
.DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \
.DMA_PeripheralInc = DMA_PeripheralInc_Disable, \
.DMA_Priority = DMA_Priority_High }
typedef uint16_t ledbuf_t;
typedef struct Color Color;
struct Color {
uint8_t R;
uint8_t G;
uint8_t B;
};
struct pios_ws2811_pin_cfg {
GPIO_TypeDef *gpio;
GPIO_InitTypeDef gpioInit;
};
struct pios_ws2811_cfg {
TIM_TimeBaseInitTypeDef timerInit;
TIM_TypeDef *timer;
uint8_t timerCh1;
uint8_t timerCh2;
DMA_InitTypeDef dmaInitCh1;
DMA_Stream_TypeDef *streamCh1;
uint32_t dmaItCh1;
DMA_InitTypeDef dmaInitCh2;
DMA_Stream_TypeDef *streamCh2;
uint32_t dmaItCh2;
DMA_InitTypeDef dmaInitUpdate;
DMA_Stream_TypeDef *streamUpdate;
uint32_t dmaItUpdate;
uint16_t dmaSource;
struct stm32_irq irq;
};
void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg);
void PIOS_WS2811_setColorRGB(Color c, uint8_t led, bool update);
void PIOS_WS2811_Update();
void PIOS_WS2811_DMA_irq_handler();
#endif /* PIOS_WS2811_H_ */

14
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 */
@ -221,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>

8
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>

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

560
flight/pios/stm32f4xx/libraries/CMSIS/Device/ST/STM32F4xx/Source/discoveryf4bare/system_stm32f4xx.c Normal file
View File

@ -0,0 +1,560 @@
/**
******************************************************************************
* @file system_stm32f4xx.c
* @author MCD Application Team
* @version V1.1.0
* @date 11-January-2013
* @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
* This file contains the system clock configuration for STM32F4xx devices,
* and is generated by the clock configuration tool
* stm32f4xx_Clock_Configuration_V1.1.0.xls
*
* 1. This file provides two functions and one global variable to be called from
* user application:
* - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
* and Divider factors, AHB/APBx prescalers and Flash settings),
* depending on the configuration made in the clock xls tool.
* This function is called at startup just after reset and
* before branch to main program. This call is made inside
* the "startup_stm32f4xx.s" file.
*
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick
* timer or configure other parameters.
*
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed
* during program execution.
*
* 2. After each device reset the HSI (16 MHz) is used as system clock source.
* Then SystemInit() function is called, in "startup_stm32f4xx.s" file, to
* configure the system clock before to branch to main program.
*
* 3. If the system clock source selected by user fails to startup, the SystemInit()
* function will do nothing and HSI still used as system clock source. User can
* add some code to deal with this issue inside the SetSysClock() function.
*
* 4. The default value of HSE crystal is set to 25MHz, refer to "HSE_VALUE" define
* in "stm32f4xx.h" file. When HSE is used as system clock source, directly or
* through PLL, and you are using different crystal you have to adapt the HSE
* value to your own configuration.
*
* 5. This file configures the system clock as follows:
*=============================================================================
*=============================================================================
* Supported STM32F40xx/41xx/427x/437x devices
*-----------------------------------------------------------------------------
* System Clock source | PLL (HSE)
*-----------------------------------------------------------------------------
* SYSCLK(Hz) | 168000000
*-----------------------------------------------------------------------------
* HCLK(Hz) | 168000000
*-----------------------------------------------------------------------------
* AHB Prescaler | 1
*-----------------------------------------------------------------------------
* APB1 Prescaler | 4
*-----------------------------------------------------------------------------
* APB2 Prescaler | 2
*-----------------------------------------------------------------------------
* HSE Frequency(Hz) | 8000000
*-----------------------------------------------------------------------------
* PLL_M | 10
*-----------------------------------------------------------------------------
* PLL_N | 420
*-----------------------------------------------------------------------------
* PLL_P | 2
*-----------------------------------------------------------------------------
* PLL_Q | 7
*-----------------------------------------------------------------------------
* PLLI2S_N | NA
*-----------------------------------------------------------------------------
* PLLI2S_R | NA
*-----------------------------------------------------------------------------
* I2S input clock | NA
*-----------------------------------------------------------------------------
* VDD(V) | 3.3
*-----------------------------------------------------------------------------
* Main regulator output voltage | Scale1 mode
*-----------------------------------------------------------------------------
* Flash Latency(WS) | 5
*-----------------------------------------------------------------------------
* Prefetch Buffer | ON
*-----------------------------------------------------------------------------
* Instruction cache | ON
*-----------------------------------------------------------------------------
* Data cache | ON
*-----------------------------------------------------------------------------
* Require 48MHz for USB OTG FS, | Enabled
* SDIO and RNG clock |
*-----------------------------------------------------------------------------
*=============================================================================
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32f4xx_system
* @{
*/
/** @addtogroup STM32F4xx_System_Private_Includes
* @{
*/
#include "stm32f4xx.h"
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_TypesDefinitions
* @{
*/
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Defines
* @{
*/
/************************* Miscellaneous Configuration ************************/
/*!< Uncomment the following line if you need to use external SRAM mounted
on STM324xG_EVAL/STM324x7I_EVAL boards as data memory */
/* #define DATA_IN_ExtSRAM */
/*!< Uncomment the following line if you need to relocate your vector Table in
Internal SRAM. */
/* #define VECT_TAB_SRAM */
#define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
/******************************************************************************/
/************************* PLL Parameters *************************************/
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */
#define PLL_M 10
#define PLL_N 420
/* SYSCLK = PLL_VCO / PLL_P */
#define PLL_P 2
/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */
#define PLL_Q 7
/******************************************************************************/
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Macros
* @{
*/
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Variables
* @{
*/
uint32_t SystemCoreClock = 168000000;
__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes
* @{
*/
static void SetSysClock(void);
#ifdef DATA_IN_ExtSRAM
static void SystemInit_ExtMemCtl(void);
#endif /* DATA_IN_ExtSRAM */
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Functions
* @{
*/
/**
* @brief Setup the microcontroller system
* Initialize the Embedded Flash Interface, the PLL and update the
* SystemFrequency variable.
* @param None
* @retval None
*/
void SystemInit(void)
{
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
#endif
/* Reset the RCC clock configuration to the default reset state ------------*/
/* Set HSION bit */
RCC->CR |= (uint32_t)0x00000001;
/* Reset CFGR register */
RCC->CFGR = 0x00000000;
/* Reset HSEON, CSSON and PLLON bits */
RCC->CR &= (uint32_t)0xFEF6FFFF;
/* Reset PLLCFGR register */
RCC->PLLCFGR = 0x24003010;
/* Reset HSEBYP bit */
RCC->CR &= (uint32_t)0xFFFBFFFF;
/* Disable all interrupts */
RCC->CIR = 0x00000000;
#ifdef DATA_IN_ExtSRAM
SystemInit_ExtMemCtl();
#endif /* DATA_IN_ExtSRAM */
/* Configure the System clock source, PLL Multiplier and Divider factors,
AHB/APBx prescalers and Flash settings ----------------------------------*/
SetSysClock();
/* Configure the Vector Table location add offset address ------------------*/
#ifdef VECT_TAB_SRAM
SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
#else
SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
#endif
}
/**
* @brief Update SystemCoreClock variable according to Clock Register Values.
* The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure
* other parameters.
*
* @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect.
*
* @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source:
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (*) HSI_VALUE is a constant defined in stm32f4xx.h file (default value
* 16 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (**) HSE_VALUE is a constant defined in stm32f4xx.h file (default value
* 25 MHz), user has to ensure that HSE_VALUE is same as the real
* frequency of the crystal used. Otherwise, this function may
* have wrong result.
*
* - The result of this function could be not correct when using fractional
* value for HSE crystal.
*
* @param None
* @retval None
*/
void SystemCoreClockUpdate(void)
{
uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;
/* Get SYSCLK source -------------------------------------------------------*/
tmp = RCC->CFGR & RCC_CFGR_SWS;
switch (tmp)
{
case 0x00: /* HSI used as system clock source */
SystemCoreClock = HSI_VALUE;
break;
case 0x04: /* HSE used as system clock source */
SystemCoreClock = HSE_VALUE;
break;
case 0x08: /* PLL used as system clock source */
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N
SYSCLK = PLL_VCO / PLL_P
*/
pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;
pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;
if (pllsource != 0)
{
/* HSE used as PLL clock source */
pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
else
{
/* HSI used as PLL clock source */
pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
SystemCoreClock = pllvco/pllp;
break;
default:
SystemCoreClock = HSI_VALUE;
break;
}
/* Compute HCLK frequency --------------------------------------------------*/
/* Get HCLK prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
/* HCLK frequency */
SystemCoreClock >>= tmp;
}
/**
* @brief Configures the System clock source, PLL Multiplier and Divider factors,
* AHB/APBx prescalers and Flash settings
* @Note This function should be called only once the RCC clock configuration
* is reset to the default reset state (done in SystemInit() function).
* @param None
* @retval None
*/
static void SetSysClock(void)
{
/******************************************************************************/
/* PLL (clocked by HSE) used as System clock source */
/******************************************************************************/
__IO uint32_t StartUpCounter = 0, HSEStatus = 0;
/* Enable HSE */
RCC->CR |= ((uint32_t)RCC_CR_HSEON);
/* Wait till HSE is ready and if Time out is reached exit */
do
{
HSEStatus = RCC->CR & RCC_CR_HSERDY;
StartUpCounter++;
} while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
HSEStatus = (uint32_t)0x01;
}
else
{
HSEStatus = (uint32_t)0x00;
}
if (HSEStatus == (uint32_t)0x01)
{
/* Select regulator voltage output Scale 1 mode, System frequency up to 168 MHz */
RCC->APB1ENR |= RCC_APB1ENR_PWREN;
PWR->CR |= PWR_CR_VOS;
/* HCLK = SYSCLK / 1*/
RCC->CFGR |= RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 2*/
RCC->CFGR |= RCC_CFGR_PPRE2_DIV2;
/* PCLK1 = HCLK / 4*/
RCC->CFGR |= RCC_CFGR_PPRE1_DIV4;
/* Configure the main PLL */
RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) |
(RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24);
/* Enable the main PLL */
RCC->CR |= RCC_CR_PLLON;
/* Wait till the main PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0)
{
}
/* Configure Flash prefetch, Instruction cache, Data cache and wait state */
FLASH->ACR = FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS;
/* Select the main PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
RCC->CFGR |= RCC_CFGR_SW_PLL;
/* Wait till the main PLL is used as system clock source */
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL);
{
}
}
else
{ /* If HSE fails to start-up, the application will have wrong clock
configuration. User can add here some code to deal with this error */
}
}
/**
* @brief Setup the external memory controller. Called in startup_stm32f4xx.s
* before jump to __main
* @param None
* @retval None
*/
#ifdef DATA_IN_ExtSRAM
/**
* @brief Setup the external memory controller.
* Called in startup_stm32f4xx.s before jump to main.
* This function configures the external SRAM mounted on STM324xG_EVAL/STM324x7I boards
* This SRAM will be used as program data memory (including heap and stack).
* @param None
* @retval None
*/
void SystemInit_ExtMemCtl(void)
{
/*-- GPIOs Configuration -----------------------------------------------------*/
/*
+-------------------+--------------------+------------------+------------------+
+ SRAM pins assignment +
+-------------------+--------------------+------------------+------------------+
| PD0 <-> FSMC_D2 | PE0 <-> FSMC_NBL0 | PF0 <-> FSMC_A0 | PG0 <-> FSMC_A10 |
| PD1 <-> FSMC_D3 | PE1 <-> FSMC_NBL1 | PF1 <-> FSMC_A1 | PG1 <-> FSMC_A11 |
| PD4 <-> FSMC_NOE | PE2 <-> FSMC_A23 | PF2 <-> FSMC_A2 | PG2 <-> FSMC_A12 |
| PD5 <-> FSMC_NWE | PE3 <-> FSMC_A19 | PF3 <-> FSMC_A3 | PG3 <-> FSMC_A13 |
| PD8 <-> FSMC_D13 | PE4 <-> FSMC_A20 | PF4 <-> FSMC_A4 | PG4 <-> FSMC_A14 |
| PD9 <-> FSMC_D14 | PE5 <-> FSMC_A21 | PF5 <-> FSMC_A5 | PG5 <-> FSMC_A15 |
| PD10 <-> FSMC_D15 | PE6 <-> FSMC_A22 | PF12 <-> FSMC_A6 | PG9 <-> FSMC_NE2 |
| PD11 <-> FSMC_A16 | PE7 <-> FSMC_D4 | PF13 <-> FSMC_A7 |------------------+
| PD12 <-> FSMC_A17 | PE8 <-> FSMC_D5 | PF14 <-> FSMC_A8 |
| PD13 <-> FSMC_A18 | PE9 <-> FSMC_D6 | PF15 <-> FSMC_A9 |
| PD14 <-> FSMC_D0 | PE10 <-> FSMC_D7 |------------------+
| PD15 <-> FSMC_D1 | PE11 <-> FSMC_D8 |
+-------------------| PE12 <-> FSMC_D9 |
| PE13 <-> FSMC_D10 |
| PE14 <-> FSMC_D11 |
| PE15 <-> FSMC_D12 |
+--------------------+
*/
/* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */
RCC->AHB1ENR |= 0x00000078;
/* Connect PDx pins to FSMC Alternate function */
GPIOD->AFR[0] = 0x00cc00cc;
GPIOD->AFR[1] = 0xcccccccc;
/* Configure PDx pins in Alternate function mode */
GPIOD->MODER = 0xaaaa0a0a;
/* Configure PDx pins speed to 100 MHz */
GPIOD->OSPEEDR = 0xffff0f0f;
/* Configure PDx pins Output type to push-pull */
GPIOD->OTYPER = 0x00000000;
/* No pull-up, pull-down for PDx pins */
GPIOD->PUPDR = 0x00000000;
/* Connect PEx pins to FSMC Alternate function */
GPIOE->AFR[0] = 0xcccccccc;
GPIOE->AFR[1] = 0xcccccccc;
/* Configure PEx pins in Alternate function mode */
GPIOE->MODER = 0xaaaaaaaa;
/* Configure PEx pins speed to 100 MHz */
GPIOE->OSPEEDR = 0xffffffff;
/* Configure PEx pins Output type to push-pull */
GPIOE->OTYPER = 0x00000000;
/* No pull-up, pull-down for PEx pins */
GPIOE->PUPDR = 0x00000000;
/* Connect PFx pins to FSMC Alternate function */
GPIOF->AFR[0] = 0x00cccccc;
GPIOF->AFR[1] = 0xcccc0000;
/* Configure PFx pins in Alternate function mode */
GPIOF->MODER = 0xaa000aaa;
/* Configure PFx pins speed to 100 MHz */
GPIOF->OSPEEDR = 0xff000fff;
/* Configure PFx pins Output type to push-pull */
GPIOF->OTYPER = 0x00000000;
/* No pull-up, pull-down for PFx pins */
GPIOF->PUPDR = 0x00000000;
/* Connect PGx pins to FSMC Alternate function */
GPIOG->AFR[0] = 0x00cccccc;
GPIOG->AFR[1] = 0x000000c0;
/* Configure PGx pins in Alternate function mode */
GPIOG->MODER = 0x00080aaa;
/* Configure PGx pins speed to 100 MHz */
GPIOG->OSPEEDR = 0x000c0fff;
/* Configure PGx pins Output type to push-pull */
GPIOG->OTYPER = 0x00000000;
/* No pull-up, pull-down for PGx pins */
GPIOG->PUPDR = 0x00000000;
/*-- FSMC Configuration ------------------------------------------------------*/
/* Enable the FSMC interface clock */
RCC->AHB3ENR |= 0x00000001;
/* Configure and enable Bank1_SRAM2 */
FSMC_Bank1->BTCR[2] = 0x00001011;
FSMC_Bank1->BTCR[3] = 0x00000201;
FSMC_Bank1E->BWTR[2] = 0x0fffffff;
/*
Bank1_SRAM2 is configured as follow:
p.FSMC_AddressSetupTime = 1;
p.FSMC_AddressHoldTime = 0;
p.FSMC_DataSetupTime = 2;
p.FSMC_BusTurnAroundDuration = 0;
p.FSMC_CLKDivision = 0;
p.FSMC_DataLatency = 0;
p.FSMC_AccessMode = FSMC_AccessMode_A;
FSMC_NORSRAMInitStructure.FSMC_Bank = FSMC_Bank1_NORSRAM2;
FSMC_NORSRAMInitStructure.FSMC_DataAddressMux = FSMC_DataAddressMux_Disable;
FSMC_NORSRAMInitStructure.FSMC_MemoryType = FSMC_MemoryType_SRAM;
FSMC_NORSRAMInitStructure.FSMC_MemoryDataWidth = FSMC_MemoryDataWidth_16b;
FSMC_NORSRAMInitStructure.FSMC_BurstAccessMode = FSMC_BurstAccessMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_AsynchronousWait = FSMC_AsynchronousWait_Disable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignalPolarity = FSMC_WaitSignalPolarity_Low;
FSMC_NORSRAMInitStructure.FSMC_WrapMode = FSMC_WrapMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignalActive = FSMC_WaitSignalActive_BeforeWaitState;
FSMC_NORSRAMInitStructure.FSMC_WriteOperation = FSMC_WriteOperation_Enable;
FSMC_NORSRAMInitStructure.FSMC_WaitSignal = FSMC_WaitSignal_Disable;
FSMC_NORSRAMInitStructure.FSMC_ExtendedMode = FSMC_ExtendedMode_Disable;
FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable;
FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &p;
FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &p;
*/
}
#endif /* DATA_IN_ExtSRAM */
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/

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

344
flight/pios/stm32f4xx/pios_ws2811.c Normal file
View File

@ -0,0 +1,344 @@
/**
******************************************************************************
*
* @file pios_ws2811.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief A driver for ws2811 rgb led controller.
* this is a plain PiOS port of the very clever solution
* implemented by Omri Iluz in the chibios driver here:
* https://github.com/omriiluz/WS2812B-LED-Driver-ChibiOS
* @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_WS2811
#include "pios_ws2811.h"
#include <stm32f4xx_rcc.h>
#include <pios_stm32.h>
#include "FreeRTOS.h"
#include "task.h"
// framebuffer
static ledbuf_t *fb = 0;
// bitmask with pin to be set/reset using dma
static ledbuf_t dmaSource[4];
static const struct pios_ws2811_cfg *pios_ws2811_cfg;
static const struct pios_ws2811_pin_cfg *pios_ws2811_pin_cfg;
static void setupTimer();
static void setupDMA();
// generic wrapper around corresponding SPL functions
static void genericTIM_OCxInit(TIM_TypeDef *TIMx, const TIM_OCInitTypeDef *TIM_OCInitStruct, uint8_t ch);
static void genericTIM_OCxPreloadConfig(TIM_TypeDef *TIMx, uint16_t TIM_OCPreload, uint8_t ch);
// timer creates a 1.25 uS signal, with duty cycle controlled by frame buffer values
/* Example configuration fragment for REVOLUTION
#ifdef PIOS_INCLUDE_WS2811
#include <pios_ws2811.h>
#include <hwsettings.h>
#define PIOS_WS2811_TIM_DIVIDER (PIOS_PERIPHERAL_APB2_CLOCK / (800000 * PIOS_WS2811_TIM_PERIOD))
// interrupt vector for DMA streamCh1
void DMA2_Stream1_IRQHandler(void) __attribute__((alias("PIOS_WS2811_irq_handler")));
const struct pios_ws2811_pin_cfg pios_ws2811_pin_cfg[] = {
[HWSETTINGS_WS2811LED_OUT_SERVOOUT1] = {
.gpio = GPIOB,
.gpioInit = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Speed = GPIO_Speed_25MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
},
},
....
[HWSETTINGS_WS2811LED_OUT_FLEXIPIN4] = {
.gpio = GPIOB,
.gpioInit = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_25MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
},
},
};
const struct pios_ws2811_cfg pios_ws2811_cfg = {
.timer = TIM1,
.timerInit = {
.TIM_Prescaler = PIOS_WS2811_TIM_DIVIDER - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
// period (1.25 uS per period
.TIM_Period = PIOS_WS2811_TIM_PERIOD,
.TIM_RepetitionCounter = 0x0000,
},
.timerCh1 = 1,
.streamCh1 = DMA2_Stream1,
.timerCh2 = 3,
.streamCh2 = DMA2_Stream6,
.streamUpdate = DMA2_Stream5,
// DMA streamCh1, triggered by timerCh1 pwm signal.
// if FrameBuffer indicates, reset output value early to indicate "0" bit to ws2812
.dmaInitCh1 = PIOS_WS2811_DMA_UPDATE_CONFIG(DMA_Channel_6),
.dmaItCh1 = DMA_IT_TEIF1 | DMA_IT_TCIF1,
// DMA streamCh2, triggered by timerCh2 pwm signal.
// Reset output value late to indicate "1" bit to ws2812.
.dmaInitCh2 = PIOS_WS2811_DMA_CH1_CONFIG(DMA_Channel_6),
.dmaItCh2 = DMA_IT_TEIF6 | DMA_IT_TCIF6,
// DMA streamUpdate Triggered by timer update event
// Outputs a high logic level at beginning of a cycle
.dmaInitUpdate = PIOS_WS2811_DMA_CH2_CONFIG(DMA_Channel_6),
.dmaItUpdate = DMA_IT_TEIF5 | DMA_IT_TCIF5,
.dmaSource = TIM_DMA_CC1 | TIM_DMA_CC3 | TIM_DMA_Update,
// DMA streamCh1 interrupt vector, used to block timer at end of framebuffer transfer
.irq = {
.flags = (DMA_IT_TCIF1),
.init = {
.NVIC_IRQChannel = DMA2_Stream1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
void PIOS_WS2811_irq_handler(void)
{
PIOS_WS2811_DMA_irq_handler();
}
#endif // PIOS_INCLUDE_WS2811
*/
/*
* How it works:
* a timer and two channels will produce the timings events:
* timer period will be 1.25us
* Ch1 CC event will be raised at 0.40uS from the beginning of the cycle
* Ch2 CC event will be raised at 0.80uS from the beginning of the cycle
* At cycle init an Update event will be raised.
*
* Three dma streams will handle the output pin as following:
* - streamUpdate dma stream, triggered by update event will produce a logic 1 on the output pin
* - streamCh1 will bring the pin to 0 if framebuffer location is set to dmaSource value to send a "0" bit to WS281x
* - streamCh2 will bring pin to 0 once .8us are passed to send a "1" bit to ws281x
* Once StreamCh1 has finished to send the buffer the IRQ handler will stop the timer.
*
*/
/**
* @brief Initialize WS2811 Led Driver
* @details Initialize the Led Driver based on passed configuration
*
* @param[in] ws2811_cfg ws2811 driver configuration
* @param[in] ws2811_pin_cfg pin to be used as output
*
*/
void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg)
{
assert_param(ws2811_cfg);
assert_param(ws2811_pin_cfg);
pios_ws2811_pin_cfg = ws2811_pin_cfg;
pios_ws2811_cfg = ws2811_cfg;
GPIO_Init(pios_ws2811_pin_cfg->gpio, &pios_ws2811_pin_cfg->gpioInit);
for (uint8_t i = 0; i < 4; i++) {
dmaSource[i] = (ledbuf_t)pios_ws2811_pin_cfg->gpioInit.GPIO_Pin;
}
fb = (ledbuf_t *)pvPortMalloc(PIOS_WS2811_BUFFER_SIZE * sizeof(ledbuf_t));
memset(fb, 0, PIOS_WS2811_BUFFER_SIZE * sizeof(ledbuf_t));
Color ledoff = { 0, 0, 0 };
for (uint8_t i = 0; i < PIOS_WS2811_NUMLEDS; i++) {
PIOS_WS2811_setColorRGB(ledoff, i, false);
}
// Setup timers
setupTimer();
setupDMA();
}
void setupTimer()
{
// Stop timer
TIM_Cmd(pios_ws2811_cfg->timer, DISABLE);
// Configure timebase and internal clock
TIM_TimeBaseInit(pios_ws2811_cfg->timer, &pios_ws2811_cfg->timerInit);
TIM_InternalClockConfig(pios_ws2811_cfg->timer);
genericTIM_OCxPreloadConfig(pios_ws2811_cfg->timer, TIM_OCPreload_Enable, pios_ws2811_cfg->timerCh1);
genericTIM_OCxPreloadConfig(pios_ws2811_cfg->timer, TIM_OCPreload_Enable, pios_ws2811_cfg->timerCh2);
TIM_ARRPreloadConfig(pios_ws2811_cfg->timer, ENABLE);
// enable outputs
// TIM_CtrlPWMOutputs(pios_ws2811_cfg->timer, ENABLE);
TIM_DMACmd(pios_ws2811_cfg->timer, pios_ws2811_cfg->dmaSource, ENABLE);
TIM_OCInitTypeDef oc = {
.TIM_OCMode = TIM_OCMode_PWM1,
.TIM_OutputState = TIM_OutputState_Enable,
.TIM_OutputNState = TIM_OutputNState_Disable,
.TIM_Pulse = 0,
.TIM_OCPolarity = TIM_OCPolarity_High,
.TIM_OCNPolarity = TIM_OCNPolarity_High,
.TIM_OCIdleState = TIM_OCIdleState_Reset,
.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
};
// (duty in ticks) / (period in ticks) * 1.25uS (period in S) = 0.40 uS
oc.TIM_Pulse = PIOS_WS2811_T0_HIGH_PERIOD * PIOS_WS2811_TIM_PERIOD / 125;
genericTIM_OCxInit(pios_ws2811_cfg->timer, &oc, pios_ws2811_cfg->timerCh1);
// (duty in ticks) / (period in ticks) * 1.25uS (period in S) = 0.80 uS
oc.TIM_Pulse = PIOS_WS2811_T1_HIGH_PERIOD * PIOS_WS2811_TIM_PERIOD / 125;
genericTIM_OCxInit(pios_ws2811_cfg->timer, &oc, pios_ws2811_cfg->timerCh2);
}
void genericTIM_OCxInit(TIM_TypeDef *TIMx, const TIM_OCInitTypeDef *TIM_OCInitStruct, uint8_t ch)
{
switch (ch) {
case 1:
TIM_OC1Init(TIMx, TIM_OCInitStruct);
break;
case 2:
TIM_OC2Init(TIMx, TIM_OCInitStruct);
break;
case 3:
TIM_OC3Init(TIMx, TIM_OCInitStruct);
break;
case 4:
TIM_OC4Init(TIMx, TIM_OCInitStruct);
break;
}
}
void genericTIM_OCxPreloadConfig(TIM_TypeDef *TIMx, uint16_t TIM_OCPreload, uint8_t ch)
{
switch (ch) {
case 1:
TIM_OC1PreloadConfig(TIMx, TIM_OCPreload);
break;
case 2:
TIM_OC2PreloadConfig(TIMx, TIM_OCPreload);
break;
case 3:
TIM_OC3PreloadConfig(TIMx, TIM_OCPreload);
break;
case 4:
TIM_OC4PreloadConfig(TIMx, TIM_OCPreload);
break;
}
}
void setupDMA()
{
// Configure Ch1
DMA_Init(pios_ws2811_cfg->streamCh1, (DMA_InitTypeDef *)&pios_ws2811_cfg->dmaInitCh1);
pios_ws2811_cfg->streamCh1->PAR = (uint32_t)&pios_ws2811_pin_cfg->gpio->BSRRH;
pios_ws2811_cfg->streamCh1->M0AR = (uint32_t)fb;
NVIC_Init((NVIC_InitTypeDef *)&(pios_ws2811_cfg->irq.init));
DMA_ITConfig(pios_ws2811_cfg->streamCh1, DMA_IT_TC, ENABLE);
DMA_Init(pios_ws2811_cfg->streamCh2, (DMA_InitTypeDef *)&pios_ws2811_cfg->dmaInitCh2);
pios_ws2811_cfg->streamCh2->PAR = (uint32_t)&pios_ws2811_pin_cfg->gpio->BSRRH;
pios_ws2811_cfg->streamCh2->M0AR = (uint32_t)dmaSource;
DMA_Init(pios_ws2811_cfg->streamUpdate, (DMA_InitTypeDef *)&pios_ws2811_cfg->dmaInitUpdate);
pios_ws2811_cfg->streamUpdate->PAR = (uint32_t)&pios_ws2811_pin_cfg->gpio->BSRRL;
pios_ws2811_cfg->streamUpdate->M0AR = (uint32_t)dmaSource;
DMA_ClearITPendingBit(pios_ws2811_cfg->streamCh1, pios_ws2811_cfg->dmaItCh1);
DMA_ClearITPendingBit(pios_ws2811_cfg->streamCh2, pios_ws2811_cfg->dmaItCh2);
DMA_ClearITPendingBit(pios_ws2811_cfg->streamUpdate, pios_ws2811_cfg->dmaItUpdate);
DMA_Cmd(pios_ws2811_cfg->streamCh2, ENABLE);
DMA_Cmd(pios_ws2811_cfg->streamCh1, ENABLE);
DMA_Cmd(pios_ws2811_cfg->streamUpdate, ENABLE);
}
void setColor(uint8_t color, ledbuf_t *buf)
{
uint8_t i;
for (i = 0; i < 8; i++) {
buf[i] = ((color << i) & 0b10000000 ? 0x0 : dmaSource[0]);
}
}
/**
* Set a led color
* @param c color
* @param led led number
* @param update Perform an update after changing led color
*/
void PIOS_WS2811_setColorRGB(Color c, uint8_t led, bool update)
{
if (led >= PIOS_WS2811_NUMLEDS) {
return;
}
setColor(c.G, fb + (led * 24));
setColor(c.R, fb + 8 + (led * 24));
setColor(c.B, fb + 16 + (led * 24));
if (update) {
PIOS_WS2811_Update();
}
}
/**
* trigger an update cycle if not already running
*/
void PIOS_WS2811_Update()
{
// does not start if framebuffer is not allocated (init has not been called yet) or a transfer is still on going
if (!fb || (pios_ws2811_cfg->timer->CR1 & TIM_CR1_CEN)) {
return;
}
// reset counters for synchronization
pios_ws2811_cfg->timer->CNT = PIOS_WS2811_TIM_PERIOD - 1;
// Start a new cycle
TIM_Cmd(pios_ws2811_cfg->timer, ENABLE);
}
/**
* Stop timer once the complete framebuffer has been sent
*/
void PIOS_WS2811_DMA_irq_handler()
{
pios_ws2811_cfg->timer->CR1 &= (uint16_t) ~TIM_CR1_CEN;
DMA_ClearFlag(pios_ws2811_cfg->streamCh1, pios_ws2811_cfg->irq.flags);
}
#endif // PIOS_INCLUDE_WS2811

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