1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-11 19:24:10 +01:00
LibrePilot/flight/libraries/sanitycheck.c

414 lines
16 KiB
C
Raw Normal View History

/**
******************************************************************************
* @addtogroup OpenPilot System OpenPilot System
* @{
* @addtogroup OpenPilot Libraries OpenPilot System Libraries
* @{
* @file sanitycheck.c
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Utilities to validate a flight configuration
* @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_board_info.h>
// Private includes
#include "inc/sanitycheck.h"
// UAVOs
#include <manualcontrolsettings.h>
#include <flightmodesettings.h>
#include <systemsettings.h>
#include <stabilizationsettings.h>
#include <systemalarms.h>
#include <revosettings.h>
#include <positionstate.h>
#include <taskinfo.h>
2014-06-01 20:28:38 +02:00
// a number of useful macros
2015-03-09 12:31:40 +01:00
#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL))
#define ADDEXTENDEDALARMSTATUS(error_code, error_substatus) if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { alarmstatus = (error_code); alarmsubstatus = (error_substatus); }
2014-06-01 20:28:38 +02:00
// private types
typedef struct SANITYCHECK_CustomHookInstance {
SANITYCHECK_CustomHook_function *hook;
struct SANITYCHECK_CustomHookInstance *next;
bool enabled;
} SANITYCHECK_CustomHookInstance;
2014-06-01 20:28:38 +02:00
// ! Check a stabilization mode switch position for safety
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted);
SANITYCHECK_CustomHookInstance *hooks = 0;
/**
* Run a preflight check over the hardware configuration
* and currently active modules
*/
int32_t configuration_check()
{
int32_t severity = SYSTEMALARMS_ALARM_OK;
SystemAlarmsExtendedAlarmStatusOptions alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
uint8_t alarmsubstatus = 0;
// Get board type
const struct pios_board_info *bdinfo = &pios_board_info_blob;
bool coptercontrol = bdinfo->board_type == 0x04;
// Classify navigation capability
#ifdef REVOLUTION
RevoSettingsInitialize();
RevoSettingsFusionAlgorithmOptions revoFusion;
RevoSettingsFusionAlgorithmGet(&revoFusion);
bool navCapableFusion;
switch (revoFusion) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF:
navCapableFusion = true;
break;
default:
navCapableFusion = false;
// check for hitl. hitl allows to feed position and velocity state via
// telemetry, this makes nav possible even with an unsuited algorithm
if (PositionStateHandle()) {
if (PositionStateReadOnly()) {
navCapableFusion = true;
}
}
}
#else /* ifdef REVOLUTION */
const bool navCapableFusion = false;
#endif /* ifdef REVOLUTION */
// Classify airframe type
bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
// For each available flight mode position sanity check the available
// modes
uint8_t num_modes;
FlightModeSettingsFlightModePositionOptions modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes);
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
FlightModeSettingsFlightModePositionGet(modes);
for (uint32_t i = 0; i < num_modes; i++) {
uint8_t gps_assisted = FlightModeAssistMap[i];
if (gps_assisted) {
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(multirotor);
ADDSEVERITY(navCapableFusion);
}
OP-1696 PathFollower rewrite in C++ and Landing Squashed commit of the following: commit 580cd8addafadc3109b90b1fd183130ea8add7f2 Author: abeck70 <abeck70@icloud.com> Date: Fri Mar 20 21:32:33 2015 +1100 OP-1696 Cleanup and bug fix 1. Code review cleanup 2. Fixed gps assist braking - the thrust limits in piddown were not being set. commit 31ac4494f6e2bc5859f49d1f76f19fe3a4d32862 Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 18 08:31:16 2015 +1100 OP-1696 fix initialisation of down pid control parameters commit a7bd737459249530bde9546b8247dd45b40dced4 Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 17 21:18:38 2015 +1100 OP-1769 Actuator fix from code review commit 968172b8fb46c91b2611be4825e17faf2fa13411 Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 17 21:01:11 2015 +1100 OP-1696 fixed ground pathfollower settings xml commit 721c07b67a87911ed67d0b62aa658cfac6d3eb97 Merge: f556215 be9f4ae Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 17 20:52:01 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit f556215ddbd5a29b22c2e6124372d74a0d6b9a0f Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 17 20:51:22 2015 +1100 OP-1696 ground pathfollower fixes commit 4584b22d6a2d33d3efb1cd11ac46804a5623a236 Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 17 18:11:52 2015 +1100 OP-1696 Ground redesign 1. Move to new PID loop design 2. Prototype steering control commit b70c3373646fa758ae30a59765b8a56df7d2fbeb Author: abeck70 <abeck70@icloud.com> Date: Sun Mar 15 18:42:06 2015 +1100 OP-1696 ground pathfollower added missing call to update velocity state commit a282bfb14dffcd486466b60be3100d26c11649ef Author: abeck70 <abeck70@icloud.com> Date: Sun Mar 15 17:22:02 2015 +1100 OP-1696 ground pathfollower improvements commit f56ea4f9b101415e32e4464e51e087ee350be969 Author: abeck70 <abeck70@icloud.com> Date: Sun Mar 15 07:44:01 2015 +1100 OP-1696 remove unused PID controller in ground for vertical commit 6424aa0a84c3718a5a7b51fab1c9e04587d325b8 Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 14 21:40:58 2015 +1100 OP-1696 Initialise vtolemergencyfallback variables commit 5d6110499bb06fe9b397e09dea5aafe994f03d13 Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 14 21:30:21 2015 +1100 OP-1696 vtolflycontroller segv fixed and uncrustify commit bef23ea3dc2b5aff5b9b8713802586719b8f68cf Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 14 20:50:05 2015 +1100 OP-1783 PathPlanner Instead of config alarm if no pathplan, simply continue to alarm pathplan, but allow normal arming and flight in other modes. If pathplan enabled, simply enter position hold. commit 370f1747209cca7c01c071a43224ad83b04bdd55 Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 14 20:48:22 2015 +1100 OP-1769 Ground code review comment 1. Remove idx1 < 0 check not needed 2. Should return abs_input in the corner case that curve is "invalid". commit 01973c656d783e035ad823c267d3387bf473eebe Merge: be07d12 5c9b6fa Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 14 12:17:31 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit be07d125f42b5ca1aabce0f5c0fcf592e8cddf98 Merge: 44afdc7 ad44b31 Author: abeck70 <abeck70@icloud.com> Date: Thu Mar 12 21:55:52 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 44afdc77987049dc58cdf42e3be88b7eae38de02 Author: abeck70 <abeck70@icloud.com> Date: Thu Mar 12 19:35:55 2015 +1100 OP-1696 fix startup Fix initialisation and void use of pios_malloc at startup commit 962f5f75756883cec6726c0347310d60264f6989 Author: abeck70 <abeck70@icloud.com> Date: Thu Mar 12 18:48:04 2015 +1100 OP-1696 undo changes to make file commit 7fa71d5c614d492cb4026312672068f92a4bcd3e Merge: 672732e 5a9cfa6 Author: abeck70 <abeck70@icloud.com> Date: Thu Mar 12 18:06:59 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 672732e95d555946300ef5093f687f3fa88bfd3a Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 11 12:59:44 2015 +1100 OP-1696 code review comments commit 73b295d6515f2489b3b3a61459c975b0c37afe3d Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 11 12:47:24 2015 +1100 OP-1696 code review comments commit 524b72f537fe922bf3c4385b418872f13819a5cf Merge: 6beef01 ba5f395 Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 10 23:19:06 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 6beef01476d06598652a10de4857e6ae55015417 Merge: 100cf33 8c31f96 Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 7 16:50:32 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 100cf335d187fe696d3678371b194fc3044a1c7e Author: Alex Beck <abeck70@icloud.com> Date: Thu Mar 5 22:32:30 2015 +1100 OP-1696 simposix c++ compilation fix commit 9ebf846d1ec8fef30ffc1204546fc5dc648ec10b Author: Alex Beck <abeck70@icloud.com> Date: Thu Mar 5 21:35:39 2015 +1100 OP-1696 fix simposix c++ compile of pathfollower commit a86b4b1db46dba569739587ee57b03460381ebab Author: abeck70 <abeck70@icloud.com> Date: Thu Mar 5 18:03:51 2015 +1100 OP-1696 resolve linux build issue commit 1d7695f81293534bc17f69b0fcd9de3876867f6a Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 4 18:10:09 2015 +1100 OP-1696 Return to Base & Land implemented Enabled via FlightModeSettings ReturnToBaseNextCommand = LAND commit 4e5ca807b18a40ea60c05c1b46f738462595a6b2 Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 4 16:38:06 2015 +1100 OP-1696 reduce memory footprint by only allocating controllers required for a particular frame type commit 78e8f559cd25ffaeac68b563e25d1ad357205fd6 Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 4 13:51:05 2015 +1100 OP-1696 move fsm initialisation to controller implementations commit 848c78a84e5c7e11b2da953c9d9549b11fc5b1d8 Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 4 13:27:11 2015 +1100 OP-1696 reduce memory through re-use of pointers to data objects in base class. commit bc8794601142055942910ba9f938c03c8a3ca547 Merge: 24bc3de 843325d Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 3 22:06:01 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 24bc3de6155a74850b17ca7706f6b592923e027c Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 3 17:56:12 2015 +1100 OP-1696 update gcs path planner options to align to new modes commit dc659891882ea0624a6436550425c54b347f4101 Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 3 13:51:55 2015 +1100 OP-1696 pathfollower class renaming and uncrustify commit af19d114b8e0ff23159bdaca37e5f5bf1c1f8beb Merge: b07c1fd 04f4cd9 Author: abeck70 <abeck70@icloud.com> Date: Mon Mar 2 21:47:12 2015 +1100 Merge branch 'rel-15.02' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit b07c1fdd0544a555678bc7a8510cd92861f17e46 Author: abeck70 <abeck70@icloud.com> Date: Mon Mar 2 21:27:27 2015 +1100 OP-1696 remainder of changes to integrate drive and fly objectives commit c824c53cf30889a468ecb25ae3d705fa011b23ba Merge: b89751c 1102243 Author: abeck70 <abeck70@icloud.com> Date: Mon Mar 2 17:06:13 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit b89751caa5d6dc06e1e20391aa4e12b5484056d4 Author: abeck70 <abeck70@icloud.com> Date: Mon Mar 2 17:04:27 2015 +1100 OP-1696 merge fly and drive pathdesired/action objectives commit c463132cb7da5cdf9fc5395ae0650b081bc9fb55 Author: abeck70 <abeck70@icloud.com> Date: Sun Mar 1 10:25:29 2015 +1100 OP-1696 merge ground pathfollower and refactored into C++ commit 6ed504b32f9e502e4f02d50b9e85ba91591417ba Merge: bcf245e deafed6 Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 27 18:12:32 2015 +1100 Merge branch 'next' into corvuscorax/OP-1642_tank-support Conflicts: flight/modules/PathFollower/pathfollower.c shared/uavobjectdefinition/vtolpathfollowersettings.xml commit bcf245e08c3348f2e6e5eddceee6d6cc268bdc23 Author: Corvus Corax <corvuscorax@cybertrench.com> Date: Sat Dec 6 19:45:29 2014 +0100 OP-1642 added new uavobject to discovery target, too commit 6b9e8a7d7dad0739b4ffd2ce7f2f78ed4f597077 Author: Corvus Corax <corvuscorax@cybertrench.com> Date: Sat Dec 6 17:58:18 2014 +0100 OP-1642 added very simple path control for ground vehicles commit e8d8440e049c06158e7fc542627fac6420abaf65 Author: Corvus Corax <corvuscorax@cybertrench.com> Date: Sat Dec 6 17:17:09 2014 +0100 OP-1642 added new settings uavobject for tank path follower commit 71dd726eb5b52614b417236c22e8a5cc3a487faf Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 27 18:05:10 2015 +1100 OP-1696 align pathaction to pathdesired commit 8a88af83be457aafbb63e4f94fad3b1cd54264ca Author: abeck70 <abeck70@icloud.com> Date: Thu Feb 26 08:06:34 2015 +1100 OP-1696 fixed wing control refactored and compiling commit fd9850b5e5680f5b422dbc468a94c5ec4b6dd518 Author: abeck70 <abeck70@icloud.com> Date: Wed Feb 25 17:47:19 2015 +1100 OP-1696 Removed vtol stuff from pathfollower.cpp after having replaced it with class impl commit 9127d8d6d8c345c5bbf8622ce4e5eb76b03d1356 Merge: 4054c73 93fb2d5 Author: abeck70 <abeck70@icloud.com> Date: Wed Feb 25 13:09:06 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 4054c73ac98b54188a0a5367c5ca913b7fee6a89 Author: abeck70 <abeck70@icloud.com> Date: Tue Feb 24 12:29:28 2015 +1100 OP-1696 fix compile errors commit b398db3d4de8cd621a059da1f0dbb00868be0fb9 Merge: 94c795a 3e15cac Author: abeck70 <abeck70@icloud.com> Date: Tue Feb 24 12:06:35 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 94c795af386cf0f1072a88e99a80cb1080bf85e7 Merge: d2115de 57e0a8b Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 23 17:54:33 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 Conflicts: flight/modules/Sensors/sensors.c make/common-defs.mk commit d2115dee4e8e9b6a02f7b7700e4715480a56b18f Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 23 14:35:02 2015 +1100 OP-1696 preliminary checkin for refactor of fly TODO: 1. Finalise and test fly, brake 2. Remove patfhollower vtol fly 3. Rename all objects adding vtol in name commit 95804b1d591d194cce3596187c74cdb2dd0d192f Merge: 58cebb0 31c1385 Author: abeck70 <abeck70@icloud.com> Date: Sat Feb 21 22:10:03 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 58cebb0658deaece1555cfa24e50f1b86fd68c96 Author: abeck70 <abeck70@icloud.com> Date: Sat Feb 21 22:09:29 2015 +1100 OP-1696 refactor neutral thrust and add pos hold implementation commit 2c3cadfef3de9e092bd66bbabc58ac7847aca7e4 Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 20 18:32:11 2015 +1100 OP-1696 start of refactor of braking commit 991c2124e834f3432f5e34c5c9f4966cb42a7be3 Merge: 1ab5798 db7d605 Author: abeck70 <abeck70@icloud.com> Date: Thu Feb 19 12:17:54 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 1ab579865f04aeaa5899f5065a983de29074e025 Author: abeck70 <abeck70@icloud.com> Date: Wed Feb 18 21:29:01 2015 +1100 OP-1696 Rewrote velocity roam into new C++ OO architecture With use of new PID controller for NE. commit 03edb049f06bf60054dce36a5b8b301a397284c4 Author: abeck70 <abeck70@icloud.com> Date: Wed Feb 18 12:19:17 2015 +1100 OP-1696 Start of refactoring of VelocityRoam implementation in PathFollower Also TODOs: 1. To support pathplanner, remove all references to FlightStatus object and add more fields to PathDesired 2. Refactor Braking to use C++ 3. Test and optimise Velocity Roam 4. Add more config options to Landing. For example disable disarming. And add more setting objects. 5. Consider moving all landing settings to one object. commit 1f5dba821d4535a9a14f333efdeec1af5afcc281 Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 16 22:51:31 2015 +1100 OP-1696 Landing tuning commit 5133956c717fba871dd5700333806e298038a3d0 Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 16 21:05:06 2015 +1100 OP-1696 Landing Multiple fixes commit cf22055ecb8eb7304174c730f249a2332ebc599b Author: abeck70 <abeck70@icloud.com> Date: Sun Feb 15 20:52:29 2015 +1100 OP-1696 Add PIDStatus debug object commit bc13d732601e7c7cb48db17f1a75b33cc96c0c34 Author: abeck70 <abeck70@icloud.com> Date: Sat Feb 14 15:53:16 2015 +1100 OP-1696 Landing sanity tested commit 6e913076c1dfcb8b90153e9eca8acf6c9e51f293 Author: abeck70 <abeck70@icloud.com> Date: Sat Feb 14 11:16:06 2015 +1100 OP-1696 completed refactoring 1. Completed refactoring of NE and Thrust control 2. Fixed velocity roam TODO Tuning and testing commit 63b62fbd0ac105029e54163b98c3beebc2d73c3a Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 13 08:54:23 2015 +1100 OP-1696 New PID Controller and refactoring of PID implementation for thrust management commit 7474331529477d0badd2ef0d609c84c556be9cec Author: abeck70 <abeck70@icloud.com> Date: Tue Feb 10 16:39:44 2015 +1100 OP-1696 Landing Refactor Thrust PID Loop for landing commit 904ab2a9241e40c19da6a1508b4ce5243c75d7a3 Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 9 22:13:39 2015 +1100 OP-1696 Uncrustify commit b3d30e298d8a0343782a29bc91be6677105b325e Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 9 22:05:05 2015 +1100 OP-1696 Create new OO design Added PathFollowerControl which has an Landing implementation. The landing implementation runs its own set of PIDs. And interfaces to the FSM for Landing. commit 1ffd8d67cf7310c1c18c90f3ba18b08221d185a3 Author: abeck70 <abeck70@icloud.com> Date: Sat Feb 7 18:04:42 2015 +1100 OP-1696 Upgrade arm tools commit 3df01f97021d13694027b92ba0b13cc9fe70d437 Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 6 21:52:38 2015 +1100 OP-1696 Landing uncrustify and start of refactoring landing to make more use of FSMLand commit 7a753b65fb69d52c89875b9a182ef5210ee8ef2d Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 6 15:08:29 2015 +1100 OP-1696 Landing: fix compilation of all exes commit 692459874acf2b7ae07aa3ab88a2c78a09f26ee6 Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 6 14:45:06 2015 +1100 OP-1696 Landing startup fixes commit 9851f29b9238a969efd0110f01a4fad6bdbc5a5d Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 6 10:21:52 2015 +1100 OP-1696 Landing pathfollower CPP refactoring commit 9b3a54d41f14f07cd0d78426026d12ae9ac4e5b5 Author: abeck70 <abeck70@icloud.com> Date: Wed Feb 4 13:02:44 2015 +1100 OP-1696 Moved to FSMLand class and fixed CPP problems 1. Additional methods, compile options, and need to have main as a cpp to solve various link issues when using static constructors but to avoid adding unnecessary libs. commit 7ebe739bfdb4e0f598f4408c1488f01bc033c044 Author: abeck70 <abeck70@icloud.com> Date: Tue Feb 3 22:17:08 2015 +1100 OP-1696 Initial Landing prototype Proof of concept. To be refactored into C++ commit 11d6754b9766efb76cc1cd57e9894badaf33ef2d Author: abeck70 <abeck70@icloud.com> Date: Tue Feb 3 22:13:10 2015 +1100 OP-1696 CPP Enablement revo c++ Enable optional revo C++ support: 1. Set USE_CXX to enable compliation and linkage of C++ source code 2. Disables rtti and exceptions 3. operator new and delete call pios malloc/free 4. Static constructor invocation supported
2015-03-20 11:42:20 +01:00
switch ((FlightModeSettingsFlightModePositionOptions)modes[i]) {
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
ADDSEVERITY(!gps_assisted);
2014-06-01 20:28:38 +02:00
ADDSEVERITY(!multirotor);
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol, gps_assisted));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol, gps_assisted));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol, gps_assisted));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol, gps_assisted));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol, gps_assisted));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol, gps_assisted));
break;
2014-06-01 20:28:38 +02:00
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
{
ADDSEVERITY(!gps_assisted);
2014-06-01 20:28:38 +02:00
}
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYROAM:
OP-1696 PathFollower rewrite in C++ and Landing Squashed commit of the following: commit 580cd8addafadc3109b90b1fd183130ea8add7f2 Author: abeck70 <abeck70@icloud.com> Date: Fri Mar 20 21:32:33 2015 +1100 OP-1696 Cleanup and bug fix 1. Code review cleanup 2. Fixed gps assist braking - the thrust limits in piddown were not being set. commit 31ac4494f6e2bc5859f49d1f76f19fe3a4d32862 Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 18 08:31:16 2015 +1100 OP-1696 fix initialisation of down pid control parameters commit a7bd737459249530bde9546b8247dd45b40dced4 Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 17 21:18:38 2015 +1100 OP-1769 Actuator fix from code review commit 968172b8fb46c91b2611be4825e17faf2fa13411 Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 17 21:01:11 2015 +1100 OP-1696 fixed ground pathfollower settings xml commit 721c07b67a87911ed67d0b62aa658cfac6d3eb97 Merge: f556215 be9f4ae Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 17 20:52:01 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit f556215ddbd5a29b22c2e6124372d74a0d6b9a0f Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 17 20:51:22 2015 +1100 OP-1696 ground pathfollower fixes commit 4584b22d6a2d33d3efb1cd11ac46804a5623a236 Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 17 18:11:52 2015 +1100 OP-1696 Ground redesign 1. Move to new PID loop design 2. Prototype steering control commit b70c3373646fa758ae30a59765b8a56df7d2fbeb Author: abeck70 <abeck70@icloud.com> Date: Sun Mar 15 18:42:06 2015 +1100 OP-1696 ground pathfollower added missing call to update velocity state commit a282bfb14dffcd486466b60be3100d26c11649ef Author: abeck70 <abeck70@icloud.com> Date: Sun Mar 15 17:22:02 2015 +1100 OP-1696 ground pathfollower improvements commit f56ea4f9b101415e32e4464e51e087ee350be969 Author: abeck70 <abeck70@icloud.com> Date: Sun Mar 15 07:44:01 2015 +1100 OP-1696 remove unused PID controller in ground for vertical commit 6424aa0a84c3718a5a7b51fab1c9e04587d325b8 Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 14 21:40:58 2015 +1100 OP-1696 Initialise vtolemergencyfallback variables commit 5d6110499bb06fe9b397e09dea5aafe994f03d13 Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 14 21:30:21 2015 +1100 OP-1696 vtolflycontroller segv fixed and uncrustify commit bef23ea3dc2b5aff5b9b8713802586719b8f68cf Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 14 20:50:05 2015 +1100 OP-1783 PathPlanner Instead of config alarm if no pathplan, simply continue to alarm pathplan, but allow normal arming and flight in other modes. If pathplan enabled, simply enter position hold. commit 370f1747209cca7c01c071a43224ad83b04bdd55 Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 14 20:48:22 2015 +1100 OP-1769 Ground code review comment 1. Remove idx1 < 0 check not needed 2. Should return abs_input in the corner case that curve is "invalid". commit 01973c656d783e035ad823c267d3387bf473eebe Merge: be07d12 5c9b6fa Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 14 12:17:31 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit be07d125f42b5ca1aabce0f5c0fcf592e8cddf98 Merge: 44afdc7 ad44b31 Author: abeck70 <abeck70@icloud.com> Date: Thu Mar 12 21:55:52 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 44afdc77987049dc58cdf42e3be88b7eae38de02 Author: abeck70 <abeck70@icloud.com> Date: Thu Mar 12 19:35:55 2015 +1100 OP-1696 fix startup Fix initialisation and void use of pios_malloc at startup commit 962f5f75756883cec6726c0347310d60264f6989 Author: abeck70 <abeck70@icloud.com> Date: Thu Mar 12 18:48:04 2015 +1100 OP-1696 undo changes to make file commit 7fa71d5c614d492cb4026312672068f92a4bcd3e Merge: 672732e 5a9cfa6 Author: abeck70 <abeck70@icloud.com> Date: Thu Mar 12 18:06:59 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 672732e95d555946300ef5093f687f3fa88bfd3a Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 11 12:59:44 2015 +1100 OP-1696 code review comments commit 73b295d6515f2489b3b3a61459c975b0c37afe3d Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 11 12:47:24 2015 +1100 OP-1696 code review comments commit 524b72f537fe922bf3c4385b418872f13819a5cf Merge: 6beef01 ba5f395 Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 10 23:19:06 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 6beef01476d06598652a10de4857e6ae55015417 Merge: 100cf33 8c31f96 Author: abeck70 <abeck70@icloud.com> Date: Sat Mar 7 16:50:32 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 100cf335d187fe696d3678371b194fc3044a1c7e Author: Alex Beck <abeck70@icloud.com> Date: Thu Mar 5 22:32:30 2015 +1100 OP-1696 simposix c++ compilation fix commit 9ebf846d1ec8fef30ffc1204546fc5dc648ec10b Author: Alex Beck <abeck70@icloud.com> Date: Thu Mar 5 21:35:39 2015 +1100 OP-1696 fix simposix c++ compile of pathfollower commit a86b4b1db46dba569739587ee57b03460381ebab Author: abeck70 <abeck70@icloud.com> Date: Thu Mar 5 18:03:51 2015 +1100 OP-1696 resolve linux build issue commit 1d7695f81293534bc17f69b0fcd9de3876867f6a Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 4 18:10:09 2015 +1100 OP-1696 Return to Base & Land implemented Enabled via FlightModeSettings ReturnToBaseNextCommand = LAND commit 4e5ca807b18a40ea60c05c1b46f738462595a6b2 Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 4 16:38:06 2015 +1100 OP-1696 reduce memory footprint by only allocating controllers required for a particular frame type commit 78e8f559cd25ffaeac68b563e25d1ad357205fd6 Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 4 13:51:05 2015 +1100 OP-1696 move fsm initialisation to controller implementations commit 848c78a84e5c7e11b2da953c9d9549b11fc5b1d8 Author: abeck70 <abeck70@icloud.com> Date: Wed Mar 4 13:27:11 2015 +1100 OP-1696 reduce memory through re-use of pointers to data objects in base class. commit bc8794601142055942910ba9f938c03c8a3ca547 Merge: 24bc3de 843325d Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 3 22:06:01 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 24bc3de6155a74850b17ca7706f6b592923e027c Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 3 17:56:12 2015 +1100 OP-1696 update gcs path planner options to align to new modes commit dc659891882ea0624a6436550425c54b347f4101 Author: abeck70 <abeck70@icloud.com> Date: Tue Mar 3 13:51:55 2015 +1100 OP-1696 pathfollower class renaming and uncrustify commit af19d114b8e0ff23159bdaca37e5f5bf1c1f8beb Merge: b07c1fd 04f4cd9 Author: abeck70 <abeck70@icloud.com> Date: Mon Mar 2 21:47:12 2015 +1100 Merge branch 'rel-15.02' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit b07c1fdd0544a555678bc7a8510cd92861f17e46 Author: abeck70 <abeck70@icloud.com> Date: Mon Mar 2 21:27:27 2015 +1100 OP-1696 remainder of changes to integrate drive and fly objectives commit c824c53cf30889a468ecb25ae3d705fa011b23ba Merge: b89751c 1102243 Author: abeck70 <abeck70@icloud.com> Date: Mon Mar 2 17:06:13 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit b89751caa5d6dc06e1e20391aa4e12b5484056d4 Author: abeck70 <abeck70@icloud.com> Date: Mon Mar 2 17:04:27 2015 +1100 OP-1696 merge fly and drive pathdesired/action objectives commit c463132cb7da5cdf9fc5395ae0650b081bc9fb55 Author: abeck70 <abeck70@icloud.com> Date: Sun Mar 1 10:25:29 2015 +1100 OP-1696 merge ground pathfollower and refactored into C++ commit 6ed504b32f9e502e4f02d50b9e85ba91591417ba Merge: bcf245e deafed6 Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 27 18:12:32 2015 +1100 Merge branch 'next' into corvuscorax/OP-1642_tank-support Conflicts: flight/modules/PathFollower/pathfollower.c shared/uavobjectdefinition/vtolpathfollowersettings.xml commit bcf245e08c3348f2e6e5eddceee6d6cc268bdc23 Author: Corvus Corax <corvuscorax@cybertrench.com> Date: Sat Dec 6 19:45:29 2014 +0100 OP-1642 added new uavobject to discovery target, too commit 6b9e8a7d7dad0739b4ffd2ce7f2f78ed4f597077 Author: Corvus Corax <corvuscorax@cybertrench.com> Date: Sat Dec 6 17:58:18 2014 +0100 OP-1642 added very simple path control for ground vehicles commit e8d8440e049c06158e7fc542627fac6420abaf65 Author: Corvus Corax <corvuscorax@cybertrench.com> Date: Sat Dec 6 17:17:09 2014 +0100 OP-1642 added new settings uavobject for tank path follower commit 71dd726eb5b52614b417236c22e8a5cc3a487faf Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 27 18:05:10 2015 +1100 OP-1696 align pathaction to pathdesired commit 8a88af83be457aafbb63e4f94fad3b1cd54264ca Author: abeck70 <abeck70@icloud.com> Date: Thu Feb 26 08:06:34 2015 +1100 OP-1696 fixed wing control refactored and compiling commit fd9850b5e5680f5b422dbc468a94c5ec4b6dd518 Author: abeck70 <abeck70@icloud.com> Date: Wed Feb 25 17:47:19 2015 +1100 OP-1696 Removed vtol stuff from pathfollower.cpp after having replaced it with class impl commit 9127d8d6d8c345c5bbf8622ce4e5eb76b03d1356 Merge: 4054c73 93fb2d5 Author: abeck70 <abeck70@icloud.com> Date: Wed Feb 25 13:09:06 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 4054c73ac98b54188a0a5367c5ca913b7fee6a89 Author: abeck70 <abeck70@icloud.com> Date: Tue Feb 24 12:29:28 2015 +1100 OP-1696 fix compile errors commit b398db3d4de8cd621a059da1f0dbb00868be0fb9 Merge: 94c795a 3e15cac Author: abeck70 <abeck70@icloud.com> Date: Tue Feb 24 12:06:35 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 94c795af386cf0f1072a88e99a80cb1080bf85e7 Merge: d2115de 57e0a8b Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 23 17:54:33 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 Conflicts: flight/modules/Sensors/sensors.c make/common-defs.mk commit d2115dee4e8e9b6a02f7b7700e4715480a56b18f Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 23 14:35:02 2015 +1100 OP-1696 preliminary checkin for refactor of fly TODO: 1. Finalise and test fly, brake 2. Remove patfhollower vtol fly 3. Rename all objects adding vtol in name commit 95804b1d591d194cce3596187c74cdb2dd0d192f Merge: 58cebb0 31c1385 Author: abeck70 <abeck70@icloud.com> Date: Sat Feb 21 22:10:03 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 58cebb0658deaece1555cfa24e50f1b86fd68c96 Author: abeck70 <abeck70@icloud.com> Date: Sat Feb 21 22:09:29 2015 +1100 OP-1696 refactor neutral thrust and add pos hold implementation commit 2c3cadfef3de9e092bd66bbabc58ac7847aca7e4 Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 20 18:32:11 2015 +1100 OP-1696 start of refactor of braking commit 991c2124e834f3432f5e34c5c9f4966cb42a7be3 Merge: 1ab5798 db7d605 Author: abeck70 <abeck70@icloud.com> Date: Thu Feb 19 12:17:54 2015 +1100 Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1696 commit 1ab579865f04aeaa5899f5065a983de29074e025 Author: abeck70 <abeck70@icloud.com> Date: Wed Feb 18 21:29:01 2015 +1100 OP-1696 Rewrote velocity roam into new C++ OO architecture With use of new PID controller for NE. commit 03edb049f06bf60054dce36a5b8b301a397284c4 Author: abeck70 <abeck70@icloud.com> Date: Wed Feb 18 12:19:17 2015 +1100 OP-1696 Start of refactoring of VelocityRoam implementation in PathFollower Also TODOs: 1. To support pathplanner, remove all references to FlightStatus object and add more fields to PathDesired 2. Refactor Braking to use C++ 3. Test and optimise Velocity Roam 4. Add more config options to Landing. For example disable disarming. And add more setting objects. 5. Consider moving all landing settings to one object. commit 1f5dba821d4535a9a14f333efdeec1af5afcc281 Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 16 22:51:31 2015 +1100 OP-1696 Landing tuning commit 5133956c717fba871dd5700333806e298038a3d0 Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 16 21:05:06 2015 +1100 OP-1696 Landing Multiple fixes commit cf22055ecb8eb7304174c730f249a2332ebc599b Author: abeck70 <abeck70@icloud.com> Date: Sun Feb 15 20:52:29 2015 +1100 OP-1696 Add PIDStatus debug object commit bc13d732601e7c7cb48db17f1a75b33cc96c0c34 Author: abeck70 <abeck70@icloud.com> Date: Sat Feb 14 15:53:16 2015 +1100 OP-1696 Landing sanity tested commit 6e913076c1dfcb8b90153e9eca8acf6c9e51f293 Author: abeck70 <abeck70@icloud.com> Date: Sat Feb 14 11:16:06 2015 +1100 OP-1696 completed refactoring 1. Completed refactoring of NE and Thrust control 2. Fixed velocity roam TODO Tuning and testing commit 63b62fbd0ac105029e54163b98c3beebc2d73c3a Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 13 08:54:23 2015 +1100 OP-1696 New PID Controller and refactoring of PID implementation for thrust management commit 7474331529477d0badd2ef0d609c84c556be9cec Author: abeck70 <abeck70@icloud.com> Date: Tue Feb 10 16:39:44 2015 +1100 OP-1696 Landing Refactor Thrust PID Loop for landing commit 904ab2a9241e40c19da6a1508b4ce5243c75d7a3 Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 9 22:13:39 2015 +1100 OP-1696 Uncrustify commit b3d30e298d8a0343782a29bc91be6677105b325e Author: abeck70 <abeck70@icloud.com> Date: Mon Feb 9 22:05:05 2015 +1100 OP-1696 Create new OO design Added PathFollowerControl which has an Landing implementation. The landing implementation runs its own set of PIDs. And interfaces to the FSM for Landing. commit 1ffd8d67cf7310c1c18c90f3ba18b08221d185a3 Author: abeck70 <abeck70@icloud.com> Date: Sat Feb 7 18:04:42 2015 +1100 OP-1696 Upgrade arm tools commit 3df01f97021d13694027b92ba0b13cc9fe70d437 Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 6 21:52:38 2015 +1100 OP-1696 Landing uncrustify and start of refactoring landing to make more use of FSMLand commit 7a753b65fb69d52c89875b9a182ef5210ee8ef2d Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 6 15:08:29 2015 +1100 OP-1696 Landing: fix compilation of all exes commit 692459874acf2b7ae07aa3ab88a2c78a09f26ee6 Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 6 14:45:06 2015 +1100 OP-1696 Landing startup fixes commit 9851f29b9238a969efd0110f01a4fad6bdbc5a5d Author: abeck70 <abeck70@icloud.com> Date: Fri Feb 6 10:21:52 2015 +1100 OP-1696 Landing pathfollower CPP refactoring commit 9b3a54d41f14f07cd0d78426026d12ae9ac4e5b5 Author: abeck70 <abeck70@icloud.com> Date: Wed Feb 4 13:02:44 2015 +1100 OP-1696 Moved to FSMLand class and fixed CPP problems 1. Additional methods, compile options, and need to have main as a cpp to solve various link issues when using static constructors but to avoid adding unnecessary libs. commit 7ebe739bfdb4e0f598f4408c1488f01bc033c044 Author: abeck70 <abeck70@icloud.com> Date: Tue Feb 3 22:17:08 2015 +1100 OP-1696 Initial Landing prototype Proof of concept. To be refactored into C++ commit 11d6754b9766efb76cc1cd57e9894badaf33ef2d Author: abeck70 <abeck70@icloud.com> Date: Tue Feb 3 22:13:10 2015 +1100 OP-1696 CPP Enablement revo c++ Enable optional revo C++ support: 1. Set USE_CXX to enable compliation and linkage of C++ source code 2. Disables rtti and exceptions 3. operator new and delete call pios malloc/free 4. Static constructor invocation supported
2015-03-20 11:42:20 +01:00
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTAKEOFF:
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(navCapableFusion);
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK:
2014-11-08 16:01:59 +01:00
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ABSOLUTEPOSITION:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
2014-06-04 10:23:37 +02:00
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
ADDSEVERITY(!gps_assisted);
2014-06-01 20:28:38 +02:00
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(navCapableFusion);
break;
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
ADDSEVERITY(!gps_assisted);
// it would be fun to try autotune on a fixed wing
// but that should only be attempted by devs at first
ADDSEVERITY(multirotor);
break;
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
default:
// Uncovered modes are automatically an error
2014-06-01 20:28:38 +02:00
ADDSEVERITY(false);
}
// mark the first encountered erroneous setting in status and substatus
if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE;
alarmsubstatus = i;
}
}
// Check throttle/collective channel range for valid configuration of input for critical control
SystemSettingsThrustControlOptions thrustType;
SystemSettingsThrustControlGet(&thrustType);
ManualControlSettingsChannelMinData channelMin;
ManualControlSettingsChannelMaxData channelMax;
ManualControlSettingsChannelMinGet(&channelMin);
ManualControlSettingsChannelMaxGet(&channelMax);
switch (thrustType) {
2015-03-09 12:31:40 +01:00
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
ADDSEVERITY(fabsf(channelMax.Throttle - channelMin.Throttle) > 300.0f);
ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
break;
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
ADDSEVERITY(fabsf(channelMax.Collective - channelMin.Collective) > 300.0f);
ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
break;
default:
break;
}
// query sanity check hooks
if (severity < SYSTEMALARMS_ALARM_CRITICAL) {
SANITYCHECK_CustomHookInstance *instance = NULL;
LL_FOREACH(hooks, instance) {
if (instance->enabled) {
alarmstatus = instance->hook();
if (alarmstatus != SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE) {
severity = SYSTEMALARMS_ALARM_CRITICAL;
break;
}
}
}
}
FlightModeSettingsDisableSanityChecksOptions checks_disabled;
FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
severity = SYSTEMALARMS_ALARM_WARNING;
}
if (severity != SYSTEMALARMS_ALARM_OK) {
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);
}
return 0;
}
/**
* Checks the stabilization settings for a particular mode and makes
* sure it is appropriate for the airframe
* @param[in] index Which stabilization mode to check
2014-06-01 20:28:38 +02:00
* @returns true or false
*/
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted)
{
uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
// Get the different axis modes for this switch position
switch (index) {
case 1:
2015-05-02 16:46:35 +02:00
FlightModeSettingsStabilization1SettingsArrayGet((FlightModeSettingsStabilization1SettingsOptions *)modes);
break;
case 2:
2015-05-02 16:46:35 +02:00
FlightModeSettingsStabilization2SettingsArrayGet((FlightModeSettingsStabilization2SettingsOptions *)modes);
break;
case 3:
2015-05-02 16:46:35 +02:00
FlightModeSettingsStabilization3SettingsArrayGet((FlightModeSettingsStabilization3SettingsOptions *)modes);
break;
case 4:
2015-05-02 16:46:35 +02:00
FlightModeSettingsStabilization4SettingsArrayGet((FlightModeSettingsStabilization4SettingsOptions *)modes);
break;
case 5:
2015-05-02 16:46:35 +02:00
FlightModeSettingsStabilization5SettingsArrayGet((FlightModeSettingsStabilization5SettingsOptions *)modes);
break;
case 6:
2015-05-02 16:46:35 +02:00
FlightModeSettingsStabilization6SettingsArrayGet((FlightModeSettingsStabilization6SettingsOptions *)modes);
break;
default:
2014-06-01 20:28:38 +02:00
return false;
}
// 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 < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
2014-06-01 20:28:38 +02:00
return false;
}
}
}
if (gpsassisted) {
// For multirotors verify that roll/pitch are either attitude or rattitude
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
if (!(modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE ||
modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) {
return false;
}
}
}
// coptercontrol cannot do altitude holding
if (coptercontrol) {
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
) {
2014-06-01 20:28:38 +02:00
return false;
}
}
// 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_ALTITUDEVARIO
) {
2014-06-01 20:28:38 +02:00
return false;
}
}
if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
)) {
2014-06-01 20:28:38 +02:00
return false;
}
// if cruise control, ensure Acro+ is not set
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL) {
2015-02-18 22:27:10 +01:00
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
// Do not allow Acro+, attitude estimation is not safe.
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ACRO) {
2015-02-18 22:27:10 +01:00
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)
2014-06-01 20:28:38 +02:00
return true;
}
FrameType_t GetCurrentFrameType()
{
SystemSettingsAirframeTypeOptions airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type);
switch ((SystemSettingsAirframeTypeOptions)airframe_type) {
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
return FRAME_TYPE_MULTIROTOR;
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
return FRAME_TYPE_FIXED_WING;
case SYSTEMSETTINGS_AIRFRAMETYPE_HELICP:
return FRAME_TYPE_HELI;
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL:
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE:
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEBOAT:
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIALBOAT:
return FRAME_TYPE_GROUND;
case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
case SYSTEMSETTINGS_AIRFRAMETYPE_CUSTOM:
return FRAME_TYPE_CUSTOM;
}
// anyway it should not reach here
return FRAME_TYPE_CUSTOM;
}
void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook)
{
PIOS_Assert(hook);
SANITYCHECK_CustomHookInstance *instance = NULL;
// Check whether there is an existing instance and enable it
LL_FOREACH(hooks, instance) {
if (instance->hook == hook) {
instance->enabled = true;
return;
}
}
// No existing instance found, attach this new one
instance = (SANITYCHECK_CustomHookInstance *)pios_malloc(sizeof(SANITYCHECK_CustomHookInstance));
PIOS_Assert(instance);
instance->hook = hook;
instance->next = NULL;
instance->enabled = true;
LL_APPEND(hooks, instance);
}
void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook)
{
if (!hooks) {
return;
}
SANITYCHECK_CustomHookInstance *instance = NULL;
LL_FOREACH(hooks, instance) {
if (instance->hook == hook) {
instance->enabled = false;
return;
}
}
}