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

OP-1696 PathFollower rewrite in C++ and Landing Squashed commit of the following:

Browse Source 
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
This commit is contained in:
abeck70 2015-03-20 21:42:20 +11:00
parent be9f4aec6a
commit 88494e2745
62 changed files with 6066 additions and 1713 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -34,6 +34,6 @@ struct path_status {
float correction_vector[3];
};
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status);
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D);
#endif

24
flight/libraries/inc/plans.h
View File

@ -63,11 +63,6 @@ void plan_setup_returnToBase();
*/
void plan_setup_land();
/**
* @brief execute land
*/
void plan_run_land();
/**
* @brief setup pathplanner/pathfollower for braking
*/
@ -78,11 +73,29 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred);
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN 2
#define PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT 3
#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH 0
#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST 1
#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN 2
#define PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED 3
#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH 0
#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST 1
#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN 2
#define PATHDESIRED_MODEPARAMETER_LAND_OPTIONS 3
#define PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE 0
#define PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH 1
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND 0
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1 1
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2 2
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3 3
/**
* @brief setup pathfollower for positionvario
*/
void plan_setup_CourseLock();
void plan_setup_PositionRoam();
void plan_setup_VelocityRoam();
void plan_setup_HomeLeash();
void plan_setup_AbsolutePosition();
@ -91,6 +104,7 @@ void plan_setup_AbsolutePosition();
*/
void plan_run_CourseLock();
void plan_run_PositionRoam();
void plan_run_VelocityRoam();
void plan_run_HomeLeash();
void plan_run_AbsolutePosition();

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

@ -143,3 +143,95 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
pid->d = d;
pid->iLim = iLim;
}
/**
* Configure the settings for a pid2 structure
* @param[out] pid The PID2 structure to configure
* @param[in] kp proportional gain
* @param[in] ki integral gain. Time constant Ti = kp/ki
* @param[in] kd derivative gain. Time constant Td = kd/kp
* @param[in] Tf filtering time = (kd/k)/N, N is in the range of 2 to 20
* @param[in] kt tracking gain for anti-windup. Tt = √TiTd and Tt = (Ti + Td)/2
* @param[in] dt delta time increment
* @param[in] beta setpoint weight on setpoint in P component. beta=1 error feedback. beta=0 smoothes out response to changes in setpoint
* @param[in] u0 initial output for r=y at activation to achieve bumpless transfer
* @param[in] va constant for compute of actuator output for check against limits for antiwindup
* @param[in] vb multiplier for compute of actuator output for check against limits for anti-windup
*/
void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb)
{
pid->reconfigure = true;
pid->u0 = u0;
pid->va = va;
pid->vb = vb;
pid->kp = kp;
pid->beta = beta; // setpoint weight on proportional term
pid->bi = ki * dT;
pid->br = kt * dT / vb;
pid->ad = Tf / (Tf + dT);
pid->bd = kd / (Tf + dT);
}
/**
* Achieve a bumpless transfer and trigger initialisation of I term
* @param[out] pid The PID structure to configure
* @param[in] u0 initial output for r=y at activation to achieve bumpless transfer
*/
void pid2_transfer(struct pid2 *pid, float u0)
{
pid->reconfigure = true;
pid->u0 = u0;
}
/**
* pid controller with setpoint weighting, anti-windup, with a low-pass filtered derivative on the process variable
* See "Feedback Systems" for an explanation
* @param[out] pid The PID structure to configure
* @param[in] r setpoint
* @param[in] y process variable
* @param[in] ulow lower limit on actuator
* @param[in] uhigh upper limit on actuator
*/
float pid2_apply(
struct pid2 *pid,
const float r,
const float y,
const float ulow,
const float uhigh)
{
// on reconfigure ensure bumpless transfer
// http://www.controlguru.com/2008/021008.html
if (pid->reconfigure) {
pid->reconfigure = false;
// initialise derivative terms
pid->yold = y;
pid->D = 0.0f;
// t=0, u=u0, y=y0, v=u
pid->I = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y);
}
// compute proportional part
pid->P = pid->kp * (pid->beta * r - y);
// update derivative part
pid->D = pid->ad * pid->D - pid->bd * (y - pid->yold);
// compute temporary output
float v = pid->va + pid->vb * (pid->P + pid->I + pid->D);
// simulate actuator saturation
float u = boundf(v, ulow, uhigh);
// update integral
pid->I = pid->I + pid->bi * (r - y) + pid->br * (u - v);
// update old process output
pid->yold = y;
return u;
}

25
flight/libraries/math/pid.h
View File

@ -44,7 +44,25 @@ struct pid {
float lastDer;
};
typedef struct pid_scaler {
// pid2 structure for a PID+setpoint weighting, anti-windup and filtered derivative control
struct pid2 {
float u0;
float va;
float vb;
float kp;
float bi;
float ad;
float bd;
float br;
float beta;
float yold;
float P;
float I;
float D;
uint8_t reconfigure;
};
typedef struct pid_scaler_s {
float p;
float i;
float d;
@ -57,4 +75,9 @@ void pid_zero(struct pid *pid);
void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
void pid_configure_derivative(float cutoff, float gamma);
// Methods for use with pid2 structure
void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb);
void pid2_transfer(struct pid2 *pid, float u0);
float pid2_apply(struct pid2 *pid, const float r, const float y, float ulow, float uhigh);
#endif /* PID_H */

28
flight/libraries/paths.c
View File

@ -44,33 +44,27 @@ static void path_circle(PathDesiredData *path, float *cur_point, struct path_sta
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status)
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
{
switch (path->Mode) {
case PATHDESIRED_MODE_BRAKE: // should never get here...
case PATHDESIRED_MODE_FLYVECTOR:
return path_vector(path, cur_point, status, true);
case PATHDESIRED_MODE_BRAKE:
case PATHDESIRED_MODE_FOLLOWVECTOR:
return path_vector(path, cur_point, status, mode3D);
break;
case PATHDESIRED_MODE_DRIVEVECTOR:
return path_vector(path, cur_point, status, false);
case PATHDESIRED_MODE_CIRCLERIGHT:
return path_circle(path, cur_point, status, mode3D);
break;
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
return path_circle(path, cur_point, status, 1);
case PATHDESIRED_MODE_CIRCLELEFT:
return path_circle(path, cur_point, status, mode3D);
break;
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
return path_circle(path, cur_point, status, 0);
case PATHDESIRED_MODE_GOTOENDPOINT:
return path_endpoint(path, cur_point, status, mode3D);
break;
case PATHDESIRED_MODE_FLYENDPOINT:
return path_endpoint(path, cur_point, status, true);
break;
case PATHDESIRED_MODE_DRIVEENDPOINT:
case PATHDESIRED_MODE_LAND:
default:
// use the endpoint as default failsafe if called in unknown modes
return path_endpoint(path, cur_point, status, false);

241
flight/libraries/plans.c
View File

@ -40,6 +40,7 @@
#include <manualcontrolcommand.h>
#include <attitudestate.h>
#include <vtolpathfollowersettings.h>
#include <stabilizationbank.h>
#include <sin_lookup.h>
#define UPDATE_EXPECTED 0.02f
@ -47,6 +48,31 @@
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
static float applyExpo(float value, float expo);
static float applyExpo(float value, float expo)
{
// note: fastPow makes a small error, therefore result needs to be bound
float exp = boundf(fastPow(1.00695f, expo), 0.5f, 2.0f);
// magic number scales expo
// so that
// expo=100 yields value**10
// expo=0 yields value**1
// expo=-100 yields value**(1/10)
// (pow(2.0,1/100)~=1.00695)
if (value > 0.0f) {
return boundf(fastPow(value, exp), 0.0f, 1.0f);
} else if (value < -0.0f) {
return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
} else {
return 0.0f;
}
}
/**
* @brief initialize UAVOs and structs used by this library
*/
@ -61,6 +87,7 @@ void plan_initialize()
ManualControlCommandInitialize();
VelocityStateInitialize();
VtolPathFollowerSettingsInitialize();
StabilizationBankInitialize();
}
/**
@ -85,7 +112,7 @@ void plan_setup_positionHold()
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(&pathDesired);
}
@ -126,49 +153,61 @@ void plan_setup_returnToBase()
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
uint8_t ReturnToBaseNextCommand;
FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand);
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1] = 0.0f;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2] = 0.0f;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3] = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(&pathDesired);
}
static PiOSDeltatimeConfig landdT;
static void plan_setup_land_helper(PathDesiredData *pathDesired)
{
PositionStateData positionState;
PositionStateGet(&positionState);
float velocity_down;
FlightModeSettingsLandingVelocityGet(&velocity_down);
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN] = velocity_down;
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_LAND;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
}
void plan_setup_land()
{
float descendspeed;
plan_setup_positionHold();
FlightModeSettingsLandingVelocityGet(&descendspeed);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.StartingVelocity = descendspeed;
pathDesired.EndingVelocity = descendspeed;
plan_setup_land_helper(&pathDesired);
PathDesiredSet(&pathDesired);
PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
}
/**
* @brief execute land
*/
void plan_run_land()
static void plan_setup_land_from_velocityroam()
{
float downPos, descendspeed;
PathDesiredEndData pathDesiredEnd;
PositionStateDownGet(&downPos); // current down position
PathDesiredEndGet(&pathDesiredEnd); // desired position
PathDesiredEndingVelocityGet(&descendspeed);
// desired position is updated to match the desired descend speed but don't run ahead
// too far if the current position can't keep up. This normaly means we have landed.
if (pathDesiredEnd.Down - downPos < 10) {
pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT);
}
PathDesiredEndSet(&pathDesiredEnd);
plan_setup_land();
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
}
/**
* @brief positionvario functionality
*/
@ -197,6 +236,14 @@ void plan_setup_PositionRoam()
plan_setup_PositionVario();
}
void plan_setup_VelocityRoam()
{
vario_control_lowpass[0] = 0.0f;
vario_control_lowpass[1] = 0.0f;
vario_control_lowpass[2] = 0.0f;
AttitudeStateYawGet(&vario_course);
}
void plan_setup_HomeLeash()
{
plan_setup_PositionVario();
@ -340,6 +387,9 @@ static void plan_run_PositionVario(vario_type type)
pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = pathDesired.End.East;
pathDesired.Start.Down = pathDesired.End.Down;
// set mode explicitly
PathDesiredSet(&pathDesired);
}
} else {
@ -381,6 +431,106 @@ static void plan_run_PositionVario(vario_type type)
}
}
void plan_run_VelocityRoam()
{
// float alpha;
PathDesiredData pathDesired;
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusFlightModeOptions flightMode;
PathDesiredGet(&pathDesired);
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
FlightStatusFlightModeGet(&flightMode);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
if (!flagRollPitchHasInput) {
// no movement desired, re-enter positionHold at current start-position
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY) {
// initiate braking and change assisted control flight mode to braking
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
// avoid brake then hold sequence to continue descent.
plan_setup_land_from_velocityroam();
} else {
plan_setup_assistedcontrol(false);
}
}
// otherwise nothing to do in braking/hold modes
} else {
PositionStateData positionState;
PositionStateGet(&positionState);
// Revert assist control state to primary, which in this case implies
// we are in roaming state (a GPS vector assisted velocity roam)
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
// Calculate desired velocity in each direction
float angle;
AttitudeStateYawGet(&angle);
angle = DEG2RAD(angle);
float cos_angle = cosf(angle);
float sine_angle = sinf(angle);
float rotated[2] = {
-cmd.Pitch * cos_angle - cmd.Roll * sine_angle,
-cmd.Pitch * sine_angle + cmd.Roll * cos_angle
};
// flip pitch to have pitch down (away) point north
float horizontalVelMax;
float verticalVelMax;
VtolPathFollowerSettingsHorizontalVelMaxGet(&horizontalVelMax);
VtolPathFollowerSettingsVerticalVelMaxGet(&verticalVelMax);
float velocity_north = rotated[0] * horizontalVelMax;
float velocity_east = rotated[1] * horizontalVelMax;
float velocity_down = 0.0f;
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
FlightModeSettingsLandingVelocityGet(&velocity_down);
}
float velocity = velocity_north * velocity_north + velocity_east * velocity_east;
velocity = sqrtf(velocity);
// if one stick input (pitch or roll) should we use fly by vector? set arbitrary distance of say 20m after which we
// expect new stick input
// if two stick input pilot is fighting wind manually and we use fly by velocity
// in reality setting velocity desired to zero will fight wind anyway.
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH] = velocity_north;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST] = velocity_east;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN] = velocity_down;
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = velocity;
pathDesired.EndingVelocity = velocity;
pathDesired.Mode = PATHDESIRED_MODE_VELOCITY;
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
pathDesired.Mode = PATHDESIRED_MODE_LAND;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE;
} else {
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED] = 0.0f;
}
PathDesiredSet(&pathDesired);
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
}
}
void plan_run_CourseLock()
{
plan_run_PositionVario(COURSE);
@ -437,7 +587,7 @@ void plan_setup_AutoCruise()
pathDesired.Start.Down = pathDesired.End.Down;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(&pathDesired);
@ -528,22 +678,27 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred)
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
if (timeout_occurred) {
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
FlightStatusFlightModeOptions flightMode;
FlightStatusFlightModeGet(&flightMode);
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
plan_setup_land_helper(&pathDesired);
} else {
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
}
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
} else {
VelocityStateData velocityState;
VelocityStateGet(&velocityState);

12
flight/libraries/sanitycheck.c
View File

@ -118,7 +118,7 @@ int32_t configuration_check()
ADDSEVERITY(navCapableFusion);
}
switch (modes[i]) {
switch ((FlightModeSettingsFlightModePositionOptions)modes[i]) {
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
ADDSEVERITY(!gps_assisted);
ADDSEVERITY(!multirotor);
@ -143,24 +143,18 @@ int32_t configuration_check()
break;
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);
ADDSEVERITY(!gps_assisted);
}
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(navCapableFusion);
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ABSOLUTEPOSITION:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:

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

@ -367,13 +367,15 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
}
if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) {
if (curve1 < 0.0f) {
curve1 = 0.0f;
float nonreversible_curve1 = curve1;
float nonreversible_curve2 = curve2;
if (nonreversible_curve1 < 0.0f) {
nonreversible_curve1 = 0.0f;
}
if (curve2 < 0.0f) {
curve2 = 0.0f;
if (nonreversible_curve2 < 0.0f) {
nonreversible_curve2 = 0.0f;
}
status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds);
status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, dTSeconds);
// If not armed or motors aren't meant to spin all the time
if (!armed ||
(!spinWhileArmed && !positiveThrottle)) {
@ -586,11 +588,7 @@ static float MixerCurveFullRangeAbsolute(const float input, const float *curve,
scale -= (float)idx1; // remainder
if (curve[0] < -1) {
return input;
}
if (idx1 < 0) {
idx1 = 0; // clamp to lowest entry in table
scale = 0;
return abs_input;
}
int idx2 = idx1 + 1;
if (idx2 >= elements) {

2
flight/modules/ManualControl/armhandler.c
View File

@ -310,6 +310,8 @@ static bool okToArm(void)
return true;
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
return false;
default:
return false;

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

@ -397,9 +397,15 @@ static void manualControlTask(void)
break;
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
// During development the assistedcontrol implementation is optional and set
// set in stabi settings. Once if we decide to always have this on, it can
// can be directly set here...i.e. set the flight mode assist as required.
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
case FLIGHTSTATUS_FLIGHTMODE_LAND:
newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
if (newFlightModeAssist) {
// Set the default thrust state
switch (newFlightModeAssist) {
case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST:
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
@ -412,28 +418,14 @@ static void manualControlTask(void)
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
break;
}
switch (newAssistedControlState) {
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE:
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
break;
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY:
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
break;
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD:
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
break;
}
}
handler = &handler_PATHFOLLOWER;
break;
case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_POI:
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
handler = &handler_PATHFOLLOWER;
@ -528,11 +520,15 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
thrustMode = modeSettings->Stabilization6Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
// we hard code the "GPS Assisted" PostionHold to use alt-vario which
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
// we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
// is a more appropriate throttle mode. "GPSAssist" adds braking
// and a better throttle management to the standard Position Hold.
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
break;
// other modes will use cruisecontrol as default
}

29
flight/modules/ManualControl/pathfollowerhandler.c
View File

@ -57,7 +57,9 @@ void pathFollowerHandler(bool newinit)
uint8_t flightMode;
uint8_t assistedControlFlightMode;
uint8_t flightModeAssist;
FlightStatusFlightModeGet(&flightMode);
FlightStatusFlightModeAssistGet(&flightModeAssist);
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
if (newinit) {
@ -67,8 +69,9 @@ void pathFollowerHandler(bool newinit)
plan_setup_returnToBase();
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
// Just initiated braking after returning from stabi control
if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) &&
(assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) {
// Switch from primary (just entered this PH flight mode) into brake
plan_setup_assistedcontrol(false);
} else {
plan_setup_positionHold();
@ -78,7 +81,11 @@ void pathFollowerHandler(bool newinit)
plan_setup_CourseLock();
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
plan_setup_PositionRoam();
if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
plan_setup_PositionRoam();
} else {
plan_setup_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
plan_setup_HomeLeash();
@ -88,7 +95,11 @@ void pathFollowerHandler(bool newinit)
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
plan_setup_land();
if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
plan_setup_land();
} else {
plan_setup_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
plan_setup_AutoCruise();
@ -117,7 +128,11 @@ void pathFollowerHandler(bool newinit)
plan_run_CourseLock();
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
plan_run_PositionRoam();
if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
plan_run_PositionRoam();
} else {
plan_run_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
plan_run_HomeLeash();
@ -126,7 +141,9 @@ void pathFollowerHandler(bool newinit)
plan_run_AbsolutePosition();
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
plan_run_land();
if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
plan_run_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
plan_run_AutoCruise();

618
flight/modules/PathFollower/fixedwingflycontroller.cpp Normal file
View File

@ -0,0 +1,618 @@
/*
******************************************************************************
*
* @file FixedWingFlyController.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Fixed wing fly controller implementation
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "fixedwingflycontroller.h"
// Private constants
// pointer to a singleton instance
FixedWingFlyController *FixedWingFlyController::p_inst = 0;
FixedWingFlyController::FixedWingFlyController()
: fixedWingSettings(NULL), mActive(false), mMode(0), indicatedAirspeedStateBias(0.0f)
{}
// Called when mode first engaged
void FixedWingFlyController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
resetGlobals();
mMode = pathDesired->Mode;
}
}
uint8_t FixedWingFlyController::IsActive(void)
{
return mActive;
}
uint8_t FixedWingFlyController::Mode(void)
{
return mMode;
}
// Objective updated in pathdesired
void FixedWingFlyController::ObjectiveUpdated(void)
{}
void FixedWingFlyController::Deactivate(void)
{
if (mActive) {
mActive = false;
resetGlobals();
}
}
void FixedWingFlyController::SettingsUpdated(void)
{
// fixed wing PID only
pid_configure(&PIDposH[0], fixedWingSettings->HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&PIDposH[1], fixedWingSettings->HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&PIDposV, fixedWingSettings->VerticalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&PIDcourse, fixedWingSettings->CoursePI.Kp, fixedWingSettings->CoursePI.Ki, 0.0f, fixedWingSettings->CoursePI.ILimit);
pid_configure(&PIDspeed, fixedWingSettings->SpeedPI.Kp, fixedWingSettings->SpeedPI.Ki, 0.0f, fixedWingSettings->SpeedPI.ILimit);
pid_configure(&PIDpower, fixedWingSettings->PowerPI.Kp, fixedWingSettings->PowerPI.Ki, 0.0f, fixedWingSettings->PowerPI.ILimit);
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t FixedWingFlyController::Initialize(FixedWingPathFollowerSettingsData *ptr_fixedWingSettings)
{
PIOS_Assert(ptr_fixedWingSettings);
fixedWingSettings = ptr_fixedWingSettings;
resetGlobals();
return 0;
}
/**
* reset integrals
*/
void FixedWingFlyController::resetGlobals()
{
pid_zero(&PIDposH[0]);
pid_zero(&PIDposH[1]);
pid_zero(&PIDposV);
pid_zero(&PIDcourse);
pid_zero(&PIDspeed);
pid_zero(&PIDpower);
pathStatus->path_time = 0.0f;
}
void FixedWingFlyController::UpdateAutoPilot()
{
uint8_t result = updateAutoPilotFixedWing();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(pathStatus);
}
/**
* fixed wing autopilot:
* straight forward:
* 1. update path velocity for limited motion crafts
* 2. update attitude according to default fixed wing pathfollower algorithm
*/
uint8_t FixedWingFlyController::updateAutoPilotFixedWing()
{
updatePathVelocity(fixedWingSettings->CourseFeedForward, true);
return updateFixedDesiredAttitude();
}
/**
* Compute desired velocity from the current position and path
*/
void FixedWingFlyController::updatePathVelocity(float kFF, bool limited)
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
// look ahead kFF seconds
float cur[3] = { positionState.North + (velocityState.North * kFF),
positionState.East + (velocityState.East * kFF),
positionState.Down + (velocityState.Down * kFF) };
struct path_status progress;
path_progress(pathDesired, cur, &progress, true);
// calculate velocity - can be zero if waypoints are too close
velocityDesired.North = progress.path_vector[0];
velocityDesired.East = progress.path_vector[1];
velocityDesired.Down = progress.path_vector[2];
if (limited &&
// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
// it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
// once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
// leading to an S-shape snake course the wrong way
// this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
// turn steep unless there is enough space complete the turn before crossing the flightpath
// 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 everything )
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
// calculating angles < 90 degrees through dot products
(vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
;
} else {
// calculate correction
velocityDesired.North += pid_apply(&PIDposH[0], progress.correction_vector[0], dT);
velocityDesired.East += pid_apply(&PIDposH[1], progress.correction_vector[1], dT);
}
velocityDesired.Down += pid_apply(&PIDposV, progress.correction_vector[2], dT);
// update pathstatus
pathStatus->error = progress.error;
pathStatus->fractional_progress = progress.fractional_progress;
pathStatus->path_direction_north = progress.path_vector[0];
pathStatus->path_direction_east = progress.path_vector[1];
pathStatus->path_direction_down = progress.path_vector[2];
pathStatus->correction_direction_north = progress.correction_vector[0];
pathStatus->correction_direction_east = progress.correction_vector[1];
pathStatus->correction_direction_down = progress.correction_vector[2];
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude from the desired velocity for fixed wing craft
*/
uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
{
uint8_t result = 1;
const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
AirspeedStateData airspeedState;
SystemSettingsData systemSettings;
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
float pitchCommand;
float descentspeedDesired;
float descentspeedError;
float powerCommand;
float airspeedVector[2];
float fluidMovement[2];
float courseComponent[2];
float courseError;
float courseCommand;
FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
VelocityStateGet(&velocityState);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* 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 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 = groundspeedProjection + indicatedAirspeedStateBias;
// 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]),
fixedWingSettings->HorizontalVelMin,
fixedWingSettings->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) &&
fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Wind = 1;
result = 0;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedWingSettings->VerticalVelMax,
fixedWingSettings->VerticalVelMax);
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
/**
* Compute desired thrust command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
-fixedWingSettings->AirspeedToPowerCrossFeed.Max,
fixedWingSettings->AirspeedToPowerCrossFeed.Max
);
// Compute final thrust response
powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedWingPathFollowerStatus.Error.Power = descentspeedError;
fixedWingPathFollowerStatus.ErrorInt.Power = PIDpower.iAccumulator;
fixedWingPathFollowerStatus.Command.Power = powerCommand;
// set thrust
stabDesired.Thrust = boundf(fixedWingSettings->ThrustLimit.Neutral + powerCommand,
fixedWingSettings->ThrustLimit.Min,
fixedWingSettings->ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedWingPathFollowerStatus.Errors.Lowpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->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
fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedWingPathFollowerStatus.Errors.Highpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->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
fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Highpower = 1;
result = 0;
}
/**
* Compute desired pitch command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
-fixedWingSettings->VerticalToPitchCrossFeed.Max,
fixedWingSettings->VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
fixedWingSettings->PitchLimit.Min,
fixedWingSettings->PitchLimit.Max);
// Error condition: high speed dive
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->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
fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
}
/**
* Compute desired roll command
*/
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
// 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 + (fixedWingSettings->ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll > 0.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f - (fixedWingSettings->ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll < 0.0f) {
courseError -= 360.0f;
}
courseCommand = pid_apply(&PIDcourse, courseError, dT);
fixedWingPathFollowerStatus.Error.Course = courseError;
fixedWingPathFollowerStatus.ErrorInt.Course = PIDcourse.iAccumulator;
fixedWingPathFollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = boundf(fixedWingSettings->RollLimit.Neutral +
courseCommand,
fixedWingSettings->RollLimit.Min,
fixedWingSettings->RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
/**
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0.0f;
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);
FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
return result;
}
/**
* 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
*/
bool FixedWingFlyController::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 = vector_lengthf(F, 2);
// normalize Cn=V/|V|, |V| must be >0
float v = vector_lengthf(V, 2);
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;
}
}
void FixedWingFlyController::AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedStateData airspeedState;
VelocityStateData velocityState;
AirspeedStateGet(&airspeedState);
VelocityStateGet(&velocityState);
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 - 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.
}

303
flight/modules/PathFollower/grounddrivecontroller.cpp Normal file
View File

@ -0,0 +1,303 @@
/*
******************************************************************************
*
* @file grounddrivecontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Ground drive controller
* the required PathDesired LAND 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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <groundpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <pathsummary.h>
#include <statusgrounddrive.h>
}
// C++ includes
#include "grounddrivecontroller.h"
// Private constants
// pointer to a singleton instance
GroundDriveController *GroundDriveController::p_inst = 0;
GroundDriveController::GroundDriveController()
: groundSettings(0), mActive(false), mMode(0)
{}
// Called when mode first engaged
void GroundDriveController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
controlNE.Activate();
mMode = pathDesired->Mode;
}
}
uint8_t GroundDriveController::IsActive(void)
{
return mActive;
}
uint8_t GroundDriveController::Mode(void)
{
return mMode;
}
// Objective updated in pathdesired
void GroundDriveController::ObjectiveUpdated(void)
{}
void GroundDriveController::Deactivate(void)
{
if (mActive) {
mActive = false;
controlNE.Deactivate();
}
}
void GroundDriveController::SettingsUpdated(void)
{
const float dT = groundSettings->UpdatePeriod / 1000.0f;
controlNE.UpdatePositionalParameters(groundSettings->HorizontalPosP);
controlNE.UpdateParameters(groundSettings->SpeedPI.Kp,
groundSettings->SpeedPI.Ki,
groundSettings->SpeedPI.Kd,
groundSettings->SpeedPI.Beta,
dT,
groundSettings->HorizontalVelMax);
// max/min are NE command values equivalent to thrust but must be symmetrical as this is NE not forward/reverse.
controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t GroundDriveController::Initialize(GroundPathFollowerSettingsData *ptr_groundSettings)
{
PIOS_Assert(ptr_groundSettings);
groundSettings = ptr_groundSettings;
return 0;
}
void GroundDriveController::UpdateAutoPilot()
{
uint8_t result = updateAutoPilotGround();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(pathStatus);
}
/**
* fixed wing autopilot:
* straight forward:
* 1. update path velocity for limited motion crafts
* 2. update attitude according to default fixed wing pathfollower algorithm
*/
uint8_t GroundDriveController::updateAutoPilotGround()
{
updatePathVelocity(groundSettings->CourseFeedForward);
return updateGroundDesiredAttitude();
}
/**
* Compute desired velocity from the current position and path
*/
void GroundDriveController::updatePathVelocity(float kFF)
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
// look ahead kFF seconds
float cur[3] = { positionState.North + (velocityState.North * kFF),
positionState.East + (velocityState.East * kFF),
positionState.Down + (velocityState.Down * kFF) };
struct path_status progress;
path_progress(pathDesired, cur, &progress, false);
// GOTOENDPOINT: correction_vector is distance array to endpoint, path_vector is velocity vector
// FOLLOWVECTOR: correct_vector is distance to vector path, path_vector is the desired velocity vector
// Calculate the desired velocity from the lateral vector path errors (correct_vector) and the desired velocity vector (path_vector)
controlNE.ControlPositionWithPath(&progress);
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
velocityDesired.Down = 0.0f;
// update pathstatus
pathStatus->error = progress.error;
pathStatus->fractional_progress = progress.fractional_progress;
// FOLLOWVECTOR: desired velocity vector
pathStatus->path_direction_north = progress.path_vector[0];
pathStatus->path_direction_east = progress.path_vector[1];
pathStatus->path_direction_down = progress.path_vector[2];
// FOLLOWVECTOR: correction distance to vector path
pathStatus->correction_direction_north = progress.correction_vector[0];
pathStatus->correction_direction_east = progress.correction_vector[1];
pathStatus->correction_direction_down = progress.correction_vector[2];
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude for ground vehicles
*/
uint8_t GroundDriveController::updateGroundDesiredAttitude()
{
StatusGroundDriveData statusGround;
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
statusGround.State.Yaw = attitudeState.Yaw;
statusGround.State.Velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// estimate a north/east command value to control the velocity error.
// ThrustLimits.Max(+-) limits the range. Think of this as a command unit vector
// of the ultimate direction to reduce lateral error and achieve the target direction (desired angle).
float northCommand, eastCommand;
controlNE.GetNECommand(&northCommand, &eastCommand);
// Get current vehicle orientation
float angle_radians = DEG2RAD(attitudeState.Yaw); // (+-pi)
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float courseCommand = 0.0f;
float speedCommand = 0.0f;
float lateralCommand = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
float forwardCommand = boundf(northCommand * cos_angle + eastCommand * sine_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
// +ve facing correct direction, lateral command should just correct angle,
if (forwardCommand > 0.0f) {
// if +ve forward command, -+ lateralCommand drives steering to manage lateral error and angular error
courseCommand = boundf(lateralCommand, -1.0f, 1.0f);
speedCommand = boundf(forwardCommand, groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.Max);
// reinstate max thrust
controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_ONTRACK;
} else {
// -ve facing opposite direction, lateral command irrelevant, need to turn to change direction and do so slowly.
// Reduce steering angle based on current velocity
float steeringReductionFactor = 1.0f;
if (stabDesired.Thrust > 0.3f) {
steeringReductionFactor = (groundSettings->HorizontalVelMax - statusGround.State.Velocity) / groundSettings->HorizontalVelMax;
steeringReductionFactor = boundf(steeringReductionFactor, 0.05f, 1.0f);
}
// should we turn left or right?
if (lateralCommand >= 0.1f) {
courseCommand = 1.0f * steeringReductionFactor;
statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDRIGHT;
} else {
courseCommand = -1.0f * steeringReductionFactor;
statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDLEFT;
}
// Impose limits to slow down.
controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.SlowForward, 0.0f);
speedCommand = groundSettings->ThrustLimit.SlowForward;
}
stabDesired.Roll = 0.0f;
stabDesired.Pitch = 0.0f;
stabDesired.Yaw = courseCommand;
// Mix yaw into thrust limit TODO
stabDesired.Thrust = boundf(speedCommand, groundSettings->ThrustLimit.Min, groundSettings->ThrustLimit.Max);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
statusGround.NECommand.North = northCommand;
statusGround.NECommand.East = eastCommand;
statusGround.State.Thrust = stabDesired.Thrust;
statusGround.BodyCommand.Forward = forwardCommand;
statusGround.BodyCommand.Right = lateralCommand;
statusGround.ControlCommand.Course = courseCommand;
statusGround.ControlCommand.Speed = speedCommand;
StatusGroundDriveSet(&statusGround);
return 1;
}

80
flight/modules/PathFollower/inc/fixedwingflycontroller.h Normal file
View File

@ -0,0 +1,80 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup FixedWing CONTROL interface class
* @brief CONTROL interface class for pathfollower fixed wing fly controller
* @{
*
* @file FixedWingCONTROL.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for fixed wing fly objectives
*
* @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 FIXEDWINGFLYCONTROLLER_H
#define FIXEDWINGFLYCONTROLLER_H
#include "pathfollowercontrol.h"
class FixedWingFlyController : public PathFollowerControl {
private:
static FixedWingFlyController *p_inst;
FixedWingFlyController();
public:
static FixedWingFlyController *instance()
{
if (!p_inst) {
p_inst = new FixedWingFlyController();
}
return p_inst;
}
int32_t Initialize(FixedWingPathFollowerSettingsData *fixedWingSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
void AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent * ev);
private:
void resetGlobals();
uint8_t updateAutoPilotFixedWing();
void updatePathVelocity(float kFF, bool limited);
uint8_t updateFixedDesiredAttitude();
bool correctCourse(float *C, float *V, float *F, float s);
FixedWingPathFollowerSettingsData *fixedWingSettings;
uint8_t mActive;
uint8_t mMode;
struct pid PIDposH[2];
struct pid PIDposV;
struct pid PIDcourse;
struct pid PIDspeed;
struct pid PIDpower;
// correct speed by measured airspeed
float indicatedAirspeedStateBias;
};
#endif // FIXEDWINGFLYCONTROLLER_H

71
flight/modules/PathFollower/inc/grounddrivecontroller.h Normal file
View File

@ -0,0 +1,71 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Ground CONTROL interface class
* @brief CONTROL interface class for ground drive controller
* @{
*
* @file grounddrivecontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Class definition for ground drive controller implementation
*
* @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 GROUNDDRIVECONTROLLER_H
#define GROUNDDRIVECONTROLLER_H
#include "pathfollowercontrol.h"
#include "pidcontrolne.h"
class GroundDriveController : public PathFollowerControl {
private:
static GroundDriveController *p_inst;
GroundDriveController();
public:
static GroundDriveController *instance()
{
if (!p_inst) {
p_inst = new GroundDriveController();
}
return p_inst;
}
int32_t Initialize(GroundPathFollowerSettingsData *groundSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
uint8_t updateAutoPilotGround();
void updatePathVelocity(float kFF);
uint8_t updateGroundDesiredAttitude();
GroundPathFollowerSettingsData *groundSettings;
uint8_t mActive;
uint8_t mMode;
PIDControlNE controlNE;
};
#endif // GROUNDDRIVECONTROLLER_H

50
flight/modules/PathFollower/inc/pathfollowercontrol.h Normal file
View File

@ -0,0 +1,50 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief CONTROL interface class for pathfollower goal implementations
* @{
*
* @file pathfollowercontrol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Interface class for controllers
*
* @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 PATHFOLLOWERCONTROL_H
#define PATHFOLLOWERCONTROL_H
class PathFollowerControl {
public:
virtual void Activate(void) = 0;
virtual void Deactivate(void) = 0;
virtual void SettingsUpdated(void) = 0;
virtual void UpdateAutoPilot(void) = 0;
virtual void ObjectiveUpdated(void) = 0;
virtual uint8_t Mode(void) = 0;
static int32_t Initialize(PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus,
PathStatusData *ptr_pathStatus);
protected:
static PathDesiredData *pathDesired;
static FlightStatusData *flightStatus;
static PathStatusData *pathStatus;
};
#endif // PATHFOLLOWERCONTROL_H

80
flight/modules/PathFollower/inc/pathfollowerfsm.h Normal file
View File

@ -0,0 +1,80 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower FSM interface class
* @brief FSM interface class for pathfollower goal fsm implementations
* @{
*
* @file pathfollowerfsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Interface class for PathFollower FSMs
*
* @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 PATHFOLLOWERFSM_H
#define PATHFOLLOWERFSM_H
extern "C" {
#include <stabilizationdesired.h>
}
typedef enum {
PFFSM_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
PFFSM_STATE_ACTIVE,
PFFSM_STATE_DISARMED,
PFFSM_STATE_ABORT // Abort on error
} PathFollowerFSMState_T;
class PathFollowerFSM {
public:
// PathFollowerFSM() {};
virtual void Inactive(void) {}
virtual void Activate(void) {}
virtual void Update(void) {}
virtual void SettingsUpdated(void) {}
virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) {}
virtual PathFollowerFSMState_T GetCurrentState(void)
{
return PFFSM_STATE_INACTIVE;
}
virtual void ConstrainStabiDesired(__attribute__((unused)) StabilizationDesiredData *stabDesired) {}
virtual float BoundVelocityDown(float velocity)
{
return velocity;
}
virtual void CheckPidScaler(__attribute__((unused)) pid_scaler *scaler) {}
virtual void GetYaw(bool &yaw_attitude, float &yaw_direction)
{
yaw_attitude = false; yaw_direction = 0.0f;
}
virtual void Abort(void) {}
virtual uint8_t ManualThrust(void)
{
return false;
}
virtual uint8_t PositionHoldState(void)
{
return false;
}
// virtual ~PathFollowerFSM() = 0;
};
#endif // PATHFOLLOWERFSM_H

95
flight/modules/PathFollower/inc/pidcontroldown.h Normal file
View File

@ -0,0 +1,95 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower PID Control implementation
* @brief PID Controller for down direction
* @{
*
* @file PIDControlDown.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes control loop for down direction
*
* @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 PIDCONTROLDOWN_H
#define PIDCONTROLDOWN_H
extern "C" {
#include <pid.h>
#include <stabilizationdesired.h>
}
#include "pathfollowerfsm.h"
class PIDControlDown {
public:
PIDControlDown();
~PIDControlDown();
void Initialize(PathFollowerFSM *fsm);
void SetThrustLimits(float min_thrust, float max_thrust);
void Deactivate();
void Activate();
void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax);
void UpdateNeutralThrust(float neutral);
void UpdateVelocitySetpoint(float setpoint);
void RateLimit(float *spDesired, float *spCurrent, float rateLimit);
void UpdateVelocityState(float pv);
float GetVelocityDesired(void);
float GetDownCommand(void);
void UpdatePositionalParameters(float kp);
void UpdatePositionState(float pvDown);
void UpdatePositionSetpoint(float setpointDown);
void ControlPosition();
void ControlPositionWithPath(struct path_status *progress);
void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
void UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate);
private:
void setup_neutralThrustCalc();
void run_neutralThrustCalc();
struct pid2 PID;
float deltaTime;
float mVelocitySetpointTarget;
float mVelocitySetpointCurrent;
float mVelocityState;
float mDownCommand;
PathFollowerFSM *mFSM;
float mNeutral;
float mVelocityMax;
struct pid PIDpos;
float mPositionSetpointTarget;
float mPositionState;
float mMinThrust;
float mMaxThrust;
uint8_t mActive;
struct NeutralThrustEstimation {
uint32_t count;
float sum;
float average;
float correction;
float min;
float max;
bool start_sampling;
bool have_correction;
};
struct NeutralThrustEstimation neutralThrustEst;
};
#endif // PIDCONTROLDOWN_H

80
flight/modules/PathFollower/inc/pidcontrolne.h Normal file
View File

@ -0,0 +1,80 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief PID Controler for NE Class definition
* @{
*
* @file pidcontrolne.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes pid control loop for NE
*
* @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 PIDCONTROLNE_H
#define PIDCONTROLNE_H
extern "C" {
#include <pid.h>
}
#include "pathfollowerfsm.h"
class PIDControlNE {
public:
PIDControlNE();
~PIDControlNE();
void Initialize();
void Deactivate();
void Activate();
void UpdateParameters(float kp, float ki, float kd, __attribute__((unused)) float ilimit, float dT, float velocityMax);
void UpdatePositionalParameters(float kp);
void UpdatePositionState(float pvNorth, float pvEast);
void UpdatePositionSetpoint(float setpointNorth, float setpointEast);
void UpdateVelocitySetpoint(float setpointNorth, float setpointEast);
// void RateLimit(float *spDesired, float *spCurrent, float rateLimit);
void UpdateVelocityState(float pvNorth, float pvEast);
void UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward);
void ControlPosition();
void ControlPositionWithPath(struct path_status *progress);
void GetNECommand(float *northCommand, float *eastCommand);
void GetVelocityDesired(float *north, float *east);
void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
void UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate);
private:
struct pid2 PIDvel[2]; // North, East
float mVelocitySetpointTarget[2];
float mVelocityState[2];
struct pid PIDposH[2];
float mPositionSetpointTarget[2];
float mPositionState[2];
float deltaTime;
float mVelocitySetpointCurrent[2];
float mNECommand;
float mNeutral;
float mVelocityMax;
float mMinCommand;
float mMaxCommand;
float mVelocityFeedforward;
uint8_t mActive;
};
#endif // PIDCONTROLNE_H

76
flight/modules/PathFollower/inc/vtolbrakecontroller.h Normal file
View File

@ -0,0 +1,76 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief CONTROL interface class for brake controller
* @{
*
* @file vtolbrakecontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes brake controller for vtol
*
* @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 VTOLBRAKECONTROLLER_H
#define VTOLBRAKECONTROLLER_H
#include "pathfollowercontrol.h"
#include "pidcontroldown.h"
#include "pidcontrolne.h"
// forward decl
class PathFollowerFSM;
class VtolBrakeController : public PathFollowerControl {
private:
static VtolBrakeController *p_inst;
VtolBrakeController();
public:
static VtolBrakeController *instance()
{
if (!p_inst) {
p_inst = new VtolBrakeController();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(void);
void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
PathFollowerFSM *fsm;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PIDControlDown controlDown;
PIDControlNE controlNE;
uint8_t mActive;
uint8_t mHoldActive;
uint8_t mManualThrust;
};
#endif // VTOLBRAKECONTROLLER_H

111
flight/modules/PathFollower/inc/vtolbrakefsm.h Normal file
View File

@ -0,0 +1,111 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower FSM Brake
* @brief Executes brake seqeuence
* @{
*
* @file vtolbrakfsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes brake sequence fsm
*
* @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 VTOLBRAKEFSM_H
#define VTOLBRAKEFSM_H
#include "pathfollowerfsm.h"
// Brakeing state machine
typedef enum {
BRAKE_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
BRAKE_STATE_BRAKE, // Initiate altitude hold before starting descent
BRAKE_STATE_HOLD, // Waiting for attainment of landing descent rate
BRAKE_STATE_SIZE
} PathFollowerFSM_BrakeState_T;
typedef enum {
FSMBRAKESTATUS_STATEEXITREASON_NONE = 0
} VtolBrakeFSMStatusStateExitReasonOptions;
class VtolBrakeFSM : public PathFollowerFSM {
private:
static VtolBrakeFSM *p_inst;
VtolBrakeFSM();
public:
static VtolBrakeFSM *instance()
{
if (!p_inst) {
p_inst = new VtolBrakeFSM();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
PathDesiredData *pathDesired,
FlightStatusData *flightStatus,
PathStatusData *ptr_pathStatus);
void Inactive(void);
void Activate(void);
void Update(void);
PathFollowerFSMState_T GetCurrentState(void);
void Abort(void);
uint8_t PositionHoldState(void);
protected:
// FSM instance data type
typedef struct {
PathFollowerFSM_BrakeState_T currentState;
uint32_t stateRunCount;
uint32_t stateTimeoutCount;
float sum1;
float sum2;
uint8_t observationCount;
uint8_t observation2Count;
} VtolBrakeFSMData_T;
// FSM state structure
typedef struct {
void(VtolBrakeFSM::*setup) (void); // Called to initialise the state
void(VtolBrakeFSM::*run) (uint8_t); // Run the event detection code for a state
} PathFollowerFSM_BrakeStateHandler_T;
// Private variables
VtolBrakeFSMData_T *mBrakeData;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PathDesiredData *pathDesired;
PathStatusData *pathStatus;
FlightStatusData *flightStatus;
void setup_brake(void);
void run_brake(uint8_t);
void initFSM(void);
void setState(PathFollowerFSM_BrakeState_T newState, VtolBrakeFSMStatusStateExitReasonOptions reason);
int32_t runState();
// void updateVtolBrakeFSMStatus();
void setStateTimeout(int32_t count);
void fallback_to_hold(void);
static PathFollowerFSM_BrakeStateHandler_T sBrakeStateTable[BRAKE_STATE_SIZE];
};
#endif // VTOLBRAKEFSM_H

84
flight/modules/PathFollower/inc/vtolflycontroller.h Normal file
View File

@ -0,0 +1,84 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief vtol fly controller class definition
* @{
*
* @file vtolflycontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes fly control for vtols
*
* @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 VTOLFLYCONTROLLER_H
#define VTOLFLYCONTROLLER_H
#include "pathfollowercontrol.h"
#include "pidcontrolne.h"
#include "pidcontroldown.h"
class VtolFlyController : public PathFollowerControl {
private:
static VtolFlyController *p_inst;
VtolFlyController();
public:
static VtolFlyController *instance()
{
if (!p_inst) {
p_inst = new VtolFlyController();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
void UpdateDesiredAttitudeEmergencyFallback();
void fallback_to_hold(void);
float updateTailInBearing();
float updateCourseBearing();
float updatePathBearing();
float updatePOIBearing();
uint8_t RunAutoPilot();
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PIDControlNE controlNE;
PIDControlDown controlDown;
uint8_t mActive;
uint8_t mManualThrust;
uint8_t mMode;
float vtolEmergencyFallback;
bool vtolEmergencyFallbackSwitch;
};
#endif // VTOLFLYCONTROLLER_H

76
flight/modules/PathFollower/inc/vtollandcontroller.h Normal file
View File

@ -0,0 +1,76 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief vtol land controller class
* @{
*
* @file vtollandcontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for landing sequence
*
* @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 VTOLLANDCONTROLLER_H
#define VTOLLANDCONTROLLER_H
#include "pathfollowercontrol.h"
#include "pidcontroldown.h"
#include "pidcontrolne.h"
// forward decl
class PathFollowerFSM;
class VtolLandController : public PathFollowerControl {
private:
static VtolLandController *p_inst;
VtolLandController();
public:
static VtolLandController *instance()
{
if (!p_inst) {
p_inst = new VtolLandController();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
void setArmedIfChanged(uint8_t val);
PathFollowerFSM *fsm;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PIDControlDown controlDown;
PIDControlNE controlNE;
uint8_t mActive;
};
#endif // VTOLLANDCONTROLLER_H

148
flight/modules/PathFollower/inc/vtollandfsm.h Normal file
View File

@ -0,0 +1,148 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower FSM
* @brief Executes landing sequence via an FSM
* @{
*
* @file vtollandfsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes FSM for landing sequence
*
* @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 VTOLLANDFSM_H
#define VTOLLANDFSM_H
extern "C" {
#include "statusvtolland.h"
}
#include "pathfollowerfsm.h"
// Landing state machine
typedef enum {
LAND_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
LAND_STATE_INIT_ALTHOLD, // Initiate altitude hold before starting descent
LAND_STATE_WTG_FOR_DESCENTRATE, // Waiting for attainment of landing descent rate
LAND_STATE_AT_DESCENTRATE, // Landing descent rate achieved
LAND_STATE_WTG_FOR_GROUNDEFFECT, // Waiting for group effect to be detected
LAND_STATE_GROUNDEFFECT, // Ground effect detected
LAND_STATE_THRUSTDOWN, // Thrust down sequence
LAND_STATE_THRUSTOFF, // Thrust is now off
LAND_STATE_DISARMED, // Disarmed
LAND_STATE_ABORT, // Abort on error triggerig fallback to hold
LAND_STATE_SIZE
} PathFollowerFSM_LandState_T;
class VtolLandFSM : public PathFollowerFSM {
private:
static VtolLandFSM *p_inst;
VtolLandFSM();
public:
static VtolLandFSM *instance()
{
if (!p_inst) {
p_inst = new VtolLandFSM();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
PathDesiredData *pathDesired,
FlightStatusData *flightStatus);
void Inactive(void);
void Activate(void);
void Update(void);
void BoundThrust(float &ulow, float &uhigh);
PathFollowerFSMState_T GetCurrentState(void);
void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
float BoundVelocityDown(float);
void CheckPidScaler(pid_scaler *scaler);
void Abort(void);
protected:
// FSM instance data type
typedef struct {
StatusVtolLandData fsmLandStatus;
PathFollowerFSM_LandState_T currentState;
TakeOffLocationData takeOffLocation;
uint32_t stateRunCount;
uint32_t stateTimeoutCount;
float sum1;
float sum2;
float expectedLandPositionNorth;
float expectedLandPositionEast;
float thrustLimit;
float boundThrustMin;
float boundThrustMax;
uint8_t observationCount;
uint8_t observation2Count;
uint8_t flZeroStabiHorizontal;
uint8_t flConstrainThrust;
uint8_t flLowAltitude;
uint8_t flAltitudeHold;
} VtolLandFSMData_T;
// FSM state structure
typedef struct {
void(VtolLandFSM::*setup) (void); // Called to initialise the state
void(VtolLandFSM::*run) (uint8_t); // Run the event detection code for a state
} PathFollowerFSM_LandStateHandler_T;
// Private variables
VtolLandFSMData_T *mLandData;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PathDesiredData *pathDesired;
FlightStatusData *flightStatus;
void setup_inactive(void);
void setup_init_althold(void);
void setup_wtg_for_descentrate(void);
void setup_at_descentrate(void);
void setup_wtg_for_groundeffect(void);
void run_init_althold(uint8_t);
void run_wtg_for_descentrate(uint8_t);
void run_at_descentrate(uint8_t);
void run_wtg_for_groundeffect(uint8_t);
void setup_groundeffect(void);
void run_groundeffect(uint8_t);
void setup_thrustdown(void);
void run_thrustdown(uint8_t);
void setup_thrustoff(void);
void run_thrustoff(uint8_t);
void setup_disarmed(void);
void run_disarmed(uint8_t);
void setup_abort(void);
void run_abort(uint8_t);
void initFSM(void);
void setState(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason);
int32_t runState();
int32_t runAlways();
void updateVtolLandFSMStatus();
void assessAltitude(void);
void setStateTimeout(int32_t count);
void fallback_to_hold(void);
static PathFollowerFSM_LandStateHandler_T sLandStateTable[LAND_STATE_SIZE];
};
#endif // VTOLLANDFSM_H

72
flight/modules/PathFollower/inc/vtolvelocitycontroller.h Normal file
View File

@ -0,0 +1,72 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower Controller
* @brief Controller for Vtol velocity roam
* @{
*
* @file vtolvelocitycontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes velocity roam control
*
* @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 PATHFOLLOWERCONTROLVELOCITYROAM_H
#define PATHFOLLOWERCONTROLVELOCITYROAM_H
#include "pathfollowercontrol.h"
#include "pidcontrolne.h"
class VtolVelocityController : public PathFollowerControl {
private:
static VtolVelocityController *p_inst;
VtolVelocityController();
public:
static VtolVelocityController *instance()
{
if (!p_inst) {
p_inst = new VtolVelocityController();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
void fallback_to_hold(void);
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PIDControlNE controlNE;
uint8_t mActive;
};
#endif // ifndef PATHFOLLOWERCONTROLVELOCITYROAM_H

1544
flight/modules/PathFollower/pathfollower.c
View File

@ -1,1544 +0,0 @@
/**
******************************************************************************
*
* @file pathfollower.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: PathDesired
* Input object: PositionState
* Input object: ManualControlCommand
* Output object: StabilizationDesired
*
* This module acts as "autopilot" - it controls the setpoints of stabilization
* based on current flight situation and desired flight path (PathDesired) as
* directed by flightmode selection or pathplanner
* This is a periodic delayed callback module
*
* 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 <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define PF_IDLE_UPDATE_RATE_MS 100
#define STACK_SIZE_BYTES 2048
#define DEADBAND_HIGH 0.10f
#define DEADBAND_LOW -0.10f
#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f
#define BRAKE_EXIT_VELOCITY_LIMIT 0.2f
#define BRAKE_RATE_MINIMUM 0.2f
#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT 0.2f
#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT 0.2f
#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT 0.1f
#define NEUTRALTHRUST_START_DELAY (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate)
#define NEUTRALTHRUST_END_COUNT (NEUTRALTHRUST_START_DELAY + (4 * 20)) // 4 second sample
// Private types
struct Globals {
struct pid PIDposH[2];
struct pid PIDposV;
struct pid PIDvel[3]; // North, East, Down
struct pid BrakePIDvel[2]; // North, East
struct pid PIDcourse;
struct pid PIDspeed;
struct pid PIDpower;
float poiRadius;
float vtolEmergencyFallback;
bool vtolEmergencyFallbackSwitch;
};
struct NeutralThrustEstimation {
uint32_t count;
float sum;
float average;
float correction;
float algo_erro_check;
float min;
float max;
bool start_sampling;
bool have_correction;
};
static struct NeutralThrustEstimation neutralThrustEst;
// Private variables
static DelayedCallbackInfo *pathFollowerCBInfo;
static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
static struct Globals global;
static PathStatusData pathStatus;
static PathDesiredData pathDesired;
static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
static VtolPathFollowerSettingsData vtolPathFollowerSettings;
static FlightStatusData flightStatus;
static PathSummaryData pathSummary;
// correct speed by measured airspeed
static float indicatedAirspeedStateBias = 0.0f;
// Private functions
static void pathFollowerTask(void);
static void resetGlobals();
static void SettingsUpdatedCb(UAVObjEvent *ev);
static uint8_t updateAutoPilotByFrameType();
static uint8_t updateAutoPilotFixedWing();
static uint8_t updateAutoPilotVtol();
static float updateTailInBearing();
static float updateCourseBearing();
static float updatePathBearing();
static float updatePOIBearing();
static void processPOI();
static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
static void updatePathVelocity(float kFF, bool limited);
static uint8_t updateFixedDesiredAttitude();
static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction);
static void updateFixedAttitude();
static void updateVtolDesiredAttitudeEmergencyFallback();
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
static bool correctCourse(float *C, float *V, float *F, float s);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t PathFollowerStart()
{
// Start main task
PathStatusGet(&pathStatus);
SettingsUpdatedCb(NULL);
PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t PathFollowerInitialize()
{
// initialize objects
FixedWingPathFollowerSettingsInitialize();
FixedWingPathFollowerStatusInitialize();
VtolPathFollowerSettingsInitialize();
FlightStatusInitialize();
FlightModeSettingsInitialize();
PathStatusInitialize();
PathSummaryInitialize();
PathDesiredInitialize();
PositionStateInitialize();
VelocityStateInitialize();
VelocityDesiredInitialize();
StabilizationDesiredInitialize();
AirspeedStateInitialize();
AttitudeStateInitialize();
TakeOffLocationInitialize();
PoiLocationInitialize();
ManualControlCommandInitialize();
SystemSettingsInitialize();
StabilizationBankInitialize();
VtolSelfTuningStatsInitialize();
// reset integrals
resetGlobals();
// Create object queue
pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
AirspeedStateConnectCallback(airspeedStateUpdatedCb);
return 0;
}
MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
/**
* Module thread, should not return.
*/
static void pathFollowerTask(void)
{
FlightStatusGet(&flightStatus);
if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
resetGlobals();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
}
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move into manualcontrol!
processPOI();
}
int16_t old_uid = pathStatus.UID;
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
if (pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
if (old_uid != pathStatus.UID) {
pathStatus.path_time = 0.0f;
} else {
pathStatus.path_time += updatePeriod / 1000.0f;
}
}
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_BRAKE:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
{
uint8_t result = updateAutoPilotByFrameType();
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);
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);
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
pid_configure(&global.PIDcourse, fixedWingPathFollowerSettings.CoursePI.Kp, fixedWingPathFollowerSettings.CoursePI.Ki, 0.0f, fixedWingPathFollowerSettings.CoursePI.ILimit);
pid_configure(&global.PIDspeed, fixedWingPathFollowerSettings.SpeedPI.Kp, fixedWingPathFollowerSettings.SpeedPI.Ki, 0.0f, fixedWingPathFollowerSettings.SpeedPI.ILimit);
pid_configure(&global.PIDpower, fixedWingPathFollowerSettings.PowerPI.Kp, fixedWingPathFollowerSettings.PowerPI.Ki, 0.0f, fixedWingPathFollowerSettings.PowerPI.ILimit);
VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit);
pid_configure(&global.BrakePIDvel[0], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit);
pid_configure(&global.BrakePIDvel[1], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit);
PathDesiredGet(&pathDesired);
}
static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedStateData airspeedState;
VelocityStateData velocityState;
AirspeedStateGet(&airspeedState);
VelocityStateGet(&velocityState);
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 - 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.
}
/**
* reset integrals
*/
static void resetGlobals()
{
pid_zero(&global.PIDposH[0]);
pid_zero(&global.PIDposH[1]);
pid_zero(&global.PIDposV);
pid_zero(&global.PIDvel[0]);
pid_zero(&global.PIDvel[1]);
pid_zero(&global.PIDvel[2]);
pid_zero(&global.BrakePIDvel[0]);
pid_zero(&global.BrakePIDvel[1]);
pid_zero(&global.PIDcourse);
pid_zero(&global.PIDspeed);
pid_zero(&global.PIDpower);
global.poiRadius = 0.0f;
global.vtolEmergencyFallback = 0;
global.vtolEmergencyFallbackSwitch = false;
// reset neutral thrust assessment. We restart this process
// and do once for each position hold engagement
neutralThrustEst.start_sampling = false;
neutralThrustEst.count = 0;
neutralThrustEst.sum = 0.0f;
neutralThrustEst.have_correction = false;
neutralThrustEst.average = 0.0f;
neutralThrustEst.correction = 0.0f;
neutralThrustEst.min = 0.0f;
neutralThrustEst.max = 0.0f;
pathStatus.path_time = 0.0f;
}
static uint8_t updateAutoPilotByFrameType()
{
FrameType_t frameType = GetCurrentFrameType();
if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) {
switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
}
}
switch (frameType) {
case FRAME_TYPE_MULTIROTOR:
case FRAME_TYPE_HELI:
updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
return updateAutoPilotVtol();
break;
case FRAME_TYPE_FIXED_WING:
default:
updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
return updateAutoPilotFixedWing();
break;
}
}
/**
* fixed wing autopilot:
* straight forward:
* 1. update path velocity for limited motion crafts
* 2. update attitude according to default fixed wing pathfollower algorithm
*/
static uint8_t updateAutoPilotFixedWing()
{
pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true);
return updateFixedDesiredAttitude();
}
/**
* vtol autopilot
* use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
* fall back to emergency fallback autopilot to keep minimum amount of flight control
*/
static uint8_t updateAutoPilotVtol()
{
enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
uint8_t result = 0;
// decide on behaviour based on settings and system state
if (global.vtolEmergencyFallbackSwitch) {
returnmode = RETURN_0;
followermode = FOLLOWER_FALLBACK;
} else {
if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
returnmode = RETURN_1;
followermode = FOLLOWER_FALLBACK;
} else {
returnmode = RETURN_RESULT;
followermode = FOLLOWER_REGULAR;
}
}
// vertical positon control PID loops works the same in both regular and fallback modes, setup according to settings
pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
switch (followermode) {
case FOLLOWER_REGULAR:
{
// horizontal position control PID loop works according to settings in regular mode, allowing integral terms
pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false);
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = true;
float yaw = 0.0f;
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
yaw_attitude = false;
} else {
switch (vtolPathFollowerSettings.YawControl) {
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
yaw_attitude = false;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
yaw = updateTailInBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
yaw = updateCourseBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
yaw = updatePathBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
yaw = updatePOIBearing();
break;
}
}
result = updateVtolDesiredAttitude(yaw_attitude, yaw);
if (!result) {
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
plan_setup_assistedcontrol(true); // revert braking to position hold, user can always stick override
} else if (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
// switch to emergency follower if follower indicates problems
global.vtolEmergencyFallbackSwitch = true;
}
}
}
break;
case FOLLOWER_FALLBACK:
{
// fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f);
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true);
// emergency follower has no return value
updateVtolDesiredAttitudeEmergencyFallback();
}
break;
}
// Brake mode end condition checks
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
bool exit_brake = false;
VelocityStateData velocityState;
if (pathStatus.path_time > pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]) { // enter hold on timeout
pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT;
exit_brake = true;
} else if (pathStatus.fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) {
VelocityStateGet(&velocityState);
if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) {
pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED;
exit_brake = true;
}
}
if (exit_brake) {
// Calculate the distance error between the originally desired
// stopping point and the actual brake-exit point.
PositionStateData p;
PositionStateGet(&p);
float north_offset = pathDesired.End.North - p.North;
float east_offset = pathDesired.End.East - p.East;
float down_offset = pathDesired.End.Down - p.Down;
pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset);
pathSummary.time_remaining = pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus.path_time;
pathSummary.fractional_progress = pathStatus.fractional_progress;
float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
cur_velocity = sqrtf(cur_velocity);
pathSummary.decelrate = (pathDesired.StartingVelocity - cur_velocity) / pathStatus.path_time;
pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings.BrakeRate;
pathSummary.velocityIntoHold = cur_velocity;
pathSummary.UID = pathStatus.UID;
PathSummarySet(&pathSummary);
plan_setup_assistedcontrol(true); // braking timeout true
// having re-entered hold allow reassessment of neutral thrust
neutralThrustEst.have_correction = false;
}
}
switch (returnmode) {
case RETURN_RESULT:
return result;
default:
// returns either 0 or 1 according to enum definition above
return returnmode;
}
}
/**
* Compute bearing of current takeoff location
*/
static float updateTailInBearing()
{
PositionStateData p;
PositionStateGet(&p);
TakeOffLocationData t;
TakeOffLocationGet(&t);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
}
/**
* Compute bearing of current movement direction
*/
static float updateCourseBearing()
{
VelocityStateData v;
VelocityStateGet(&v);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(v.East, v.North));
}
/**
* Compute bearing of current path direction
*/
static float updatePathBearing()
{
PositionStateData positionState;
PositionStateGet(&positionState);
float cur[3] = { positionState.North,
positionState.East,
positionState.Down };
struct path_status progress;
path_progress(&pathDesired, cur, &progress);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
}
/**
* Compute bearing between current position and POI
*/
static float updatePOIBearing()
{
PoiLocationData poi;
PoiLocationGet(&poi);
PositionStateData positionState;
PositionStateGet(&positionState);
const float dT = updatePeriod / 1000.0f;
float dLoc[3];
float yaw = 0;
/*float elevation = 0;*/
dLoc[0] = positionState.North - poi.North;
dLoc[1] = positionState.East - poi.East;
dLoc[2] = positionState.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
}
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
}
return yaw + (pathAngle / 2.0f);
}
/**
* process POI control logic TODO: this should most likely go into manualcontrol!!!!
* TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently
**/
static void processPOI()
{
const float dT = updatePeriod / 1000.0f;
PositionStateData positionState;
PositionStateGet(&positionState);
// TODO put commented out camera feature code back in place either
// permanently or optionally or remove it
// CameraDesiredData cameraDesired;
// CameraDesiredGet(&cameraDesired);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
PoiLocationData poi;
PoiLocationGet(&poi);
float dLoc[3];
float yaw = 0;
// TODO camera feature
/*float elevation = 0;*/
dLoc[0] = positionState.North - poi.North;
dLoc[1] = positionState.East - poi.East;
dLoc[2] = positionState.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
}
// distance
float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
float changeRadius = 0;
// Move closer or further, radially
if (manualControlData.Pitch > DEADBAND_HIGH) {
changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
} else if (manualControlData.Pitch < DEADBAND_LOW) {
changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
}
// move along circular path
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
} else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
// change radius only when not circling
global.poiRadius = distance + changeRadius;
}
// don't try to move any closer
if (global.poiRadius >= 3.0f || changeRadius > 0) {
if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
pathDesired.End.North = poi.North + (global.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End.East = poi.East + (global.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.StartingVelocity = 1.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
}
// not above
if (distance >= 3.0f) {
// TODO camera feature
// You can feed this into camerastabilization
/*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
// cameraDesired.Yaw=yaw;
// cameraDesired.PitchOrServo2=elevation;
// CameraDesiredSet(&cameraDesired);
}
}
static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
{
if (startingVelocity >= 0.0f) {
*updatedVelocity = startingVelocity - dT * brakeRate;
if (*updatedVelocity > currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity < 0.0f) {
*updatedVelocity = 0.0f;
}
} else {
*updatedVelocity = startingVelocity + dT * brakeRate;
if (*updatedVelocity < currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity > 0.0f) {
*updatedVelocity = 0.0f;
}
}
}
/**
* Compute desired velocity from the current position and path
*/
static void updatePathVelocity(float kFF, bool limited)
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
const float dT = updatePeriod / 1000.0f;
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
float brakeRate = vtolPathFollowerSettings.BrakeRate;
if (brakeRate < BRAKE_RATE_MINIMUM) {
brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
}
updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH], pathStatus.path_time, brakeRate, velocityState.North, &velocityDesired.North);
updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST], pathStatus.path_time, brakeRate, velocityState.East, &velocityDesired.East);
updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN], pathStatus.path_time, brakeRate, velocityState.Down, &velocityDesired.Down);
float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
cur_velocity = sqrtf(cur_velocity);
float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down;
desired_velocity = sqrtf(desired_velocity);
// update pathstatus
pathStatus.error = cur_velocity - desired_velocity;
pathStatus.fractional_progress = 1.0f;
if (pathDesired.StartingVelocity > 0.0f) {
pathStatus.fractional_progress = (pathDesired.StartingVelocity - cur_velocity) / pathDesired.StartingVelocity;
// sometimes current velocity can exceed starting velocity due to initial acceleration
if (pathStatus.fractional_progress < 0.0f) {
pathStatus.fractional_progress = 0.0f;
}
}
pathStatus.path_direction_north = velocityDesired.North;
pathStatus.path_direction_east = velocityDesired.East;
pathStatus.path_direction_down = velocityDesired.Down;
pathStatus.correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus.correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus.correction_direction_down = velocityDesired.Down - velocityState.Down;
} else {
// look ahead kFF seconds
float cur[3] = { positionState.North + (velocityState.North * kFF),
positionState.East + (velocityState.East * kFF),
positionState.Down + (velocityState.Down * kFF) };
struct path_status progress;
path_progress(&pathDesired, cur, &progress);
// calculate velocity - can be zero if waypoints are too close
velocityDesired.North = progress.path_vector[0];
velocityDesired.East = progress.path_vector[1];
velocityDesired.Down = progress.path_vector[2];
if (limited &&
// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
// it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
// once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
// leading to an S-shape snake course the wrong way
// this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
// turn steep unless there is enough space complete the turn before crossing the flightpath
// 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 everything )
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
// calculating angles < 90 degrees through dot products
(vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
;
} else {
// calculate correction
velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT);
velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT);
}
velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT);
// update pathstatus
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
pathStatus.path_direction_north = progress.path_vector[0];
pathStatus.path_direction_east = progress.path_vector[1];
pathStatus.path_direction_down = progress.path_vector[2];
pathStatus.correction_direction_north = progress.correction_vector[0];
pathStatus.correction_direction_east = progress.correction_vector[1];
pathStatus.correction_direction_down = progress.correction_vector[2];
}
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude from the desired velocity for fixed wing craft
*/
static uint8_t updateFixedDesiredAttitude()
{
uint8_t result = 1;
const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s]
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
AirspeedStateData airspeedState;
SystemSettingsData systemSettings;
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
float pitchCommand;
float descentspeedDesired;
float descentspeedError;
float powerCommand;
float airspeedVector[2];
float fluidMovement[2];
float courseComponent[2];
float courseError;
float courseCommand;
FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
VelocityStateGet(&velocityState);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* 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 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 = groundspeedProjection + indicatedAirspeedStateBias;
// 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 = boundf(
velocityDesired.Down,
-fixedWingPathFollowerSettings.VerticalVelMax,
fixedWingPathFollowerSettings.VerticalVelMax);
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) {
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) {
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) {
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
/**
* Compute desired thrust command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp,
-fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max,
fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max
);
// Compute final thrust response
powerCommand = pid_apply(&global.PIDpower, -descentspeedError, dT) +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedWingPathFollowerStatus.Error.Power = descentspeedError;
fixedWingPathFollowerStatus.ErrorInt.Power = global.PIDpower.iAccumulator;
fixedWingPathFollowerStatus.Command.Power = powerCommand;
// 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 (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 thrust (opposite of above)
fixedWingPathFollowerStatus.Errors.Highpower = 0;
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;
}
/**
* Compute desired pitch command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
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 = -pid_apply(&global.PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
fixedWingPathFollowerStatus.ErrorInt.Speed = global.PIDspeed.iAccumulator;
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedWingPathFollowerSettings.PitchLimit.Min,
fixedWingPathFollowerSettings.PitchLimit.Max);
// Error condition: high speed dive
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
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;
}
/**
* Compute desired roll command
*/
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
// 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;
}
courseCommand = pid_apply(&global.PIDcourse, courseError, dT);
fixedWingPathFollowerStatus.Error.Course = courseError;
fixedWingPathFollowerStatus.ErrorInt.Course = global.PIDcourse.iAccumulator;
fixedWingPathFollowerStatus.Command.Course = courseCommand;
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
/**
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0.0f;
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);
FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
return result;
}
/**
* 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 = vector_lengthf(F, 2);
// normalize Cn=V/|V|, |V| must be >0
float v = vector_lengthf(V, 2);
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;
}
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedState which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityState against the @ref VelocityDesired
*/
static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
{
const float dT = updatePeriod / 1000.0f;
uint8_t result = 1;
bool manual_thrust = false;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
SystemSettingsData systemSettings;
VtolSelfTuningStatsData vtolSelfTuningStats;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
SystemSettingsGet(&systemSettings);
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
// scale velocity if it is above configured maximum
// for braking, we can not help it if initial velocity was greater
float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
if (velH > vtolPathFollowerSettings.HorizontalVelMax) {
velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH;
velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH;
}
if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) {
velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down);
}
}
// calculate the velocity errors between desired and actual
northError = velocityDesired.North - velocityState.North;
eastError = velocityDesired.East - velocityState.East;
downError = velocityDesired.Down - velocityState.Down;
// Must flip this sign
downError = -downError;
// Compute desired commands
if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward;
eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward;
} else {
northCommand = pid_apply(&global.BrakePIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.BrakeVelocityFeedforward;
eastCommand = pid_apply(&global.BrakePIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.BrakeVelocityFeedforward;
}
downCommand = pid_apply(&global.PIDvel[2], downError, dT);
if ((vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL &&
flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) ||
(flightStatus.FlightModeAssist && flightStatus.AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) {
manual_thrust = true;
}
// if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment
// note that arming into this flight mode is not allowed, so assumption here is that
// we have not landed. If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the
// altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL.
// In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min,
// avoiding auto-takeoffs. Therefore no need to check that here.
if (!manual_thrust && neutralThrustEst.have_correction != true) {
// Assess if position hold state running. This can be normal position hold or
// another mode with assist-hold active.
bool ph_active =
((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD &&
flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) ||
(flightStatus.FlightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE &&
flightStatus.AssistedControlState == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD));
bool stable = (fabsf(pathStatus.correction_direction_down) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT &&
fabsf(velocityDesired.Down) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT &&
fabsf(velocityState.Down) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT &&
fabsf(downError) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT);
if (ph_active && stable) {
if (neutralThrustEst.start_sampling) {
neutralThrustEst.count++;
// delay count for 2 seconds into hold allowing for stablisation
if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) {
neutralThrustEst.sum += global.PIDvel[2].iAccumulator;
if (global.PIDvel[2].iAccumulator < neutralThrustEst.min) {
neutralThrustEst.min = global.PIDvel[2].iAccumulator;
}
if (global.PIDvel[2].iAccumulator > neutralThrustEst.max) {
neutralThrustEst.max = global.PIDvel[2].iAccumulator;
}
}
if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) {
// 6 seconds have past
// lets take an average
neutralThrustEst.average = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY);
neutralThrustEst.correction = neutralThrustEst.average / 1000.0f;
global.PIDvel[2].iAccumulator -= neutralThrustEst.average;
neutralThrustEst.start_sampling = false;
neutralThrustEst.have_correction = true;
// Write a new adjustment value
// vtolSelfTuningStats.NeutralThrustOffset was incremental adjusted above
VtolSelfTuningStatsData new_vtolSelfTuningStats;
// add the average remaining i value to the
new_vtolSelfTuningStats.NeutralThrustOffset = vtolSelfTuningStats.NeutralThrustOffset + neutralThrustEst.correction;
new_vtolSelfTuningStats.NeutralThrustCorrection = neutralThrustEst.correction; // the i term thrust correction value applied
new_vtolSelfTuningStats.NeutralThrustAccumulator = global.PIDvel[2].iAccumulator; // the actual iaccumulator value after correction
new_vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
VtolSelfTuningStatsSet(&new_vtolSelfTuningStats);
}
} else {
// start a tick count
neutralThrustEst.start_sampling = true;
neutralThrustEst.count = 0;
neutralThrustEst.sum = 0.0f;
neutralThrustEst.max = 0.0f;
neutralThrustEst.min = 0.0f;
}
} else {
// reset sampling as we did't get 6 continuous seconds
neutralThrustEst.start_sampling = false;
}
} // else we already have a correction for this PH run
// Generally in braking the downError will be an increased altitude. We really will rely on cruisecontrol to backoff.
stabDesired.Thrust = boundf(vtolSelfTuningStats.NeutralThrustOffset + downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
// DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
attitudeState.Yaw += 120.0f;
if (attitudeState.Yaw > 180.0f) {
attitudeState.Yaw -= 360.0f;
}
}
if ( // emergency flyaway detection
( // integral already at its limit
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
) &&
// angle between desired and actual velocity >90 degrees (by dot product)
(velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
// quad is moving at significant speed (during flyaway it would keep speeding up)
squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
) {
global.vtolEmergencyFallback += dT;
if (global.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) {
// after emergency timeout, trigger alarm - everything else is handled by callers
// (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
result = 0;
}
} else {
global.vtolEmergencyFallback = 0.0f;
}
// 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
float maxPitch = vtolPathFollowerSettings.MaxRollPitch;
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
maxPitch = vtolPathFollowerSettings.BrakeMaxPitch;
}
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-maxPitch, maxPitch);
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
// when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode.
if (flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) {
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
thrustMode = settings.Stabilization1Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
thrustMode = settings.Stabilization2Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
thrustMode = settings.Stabilization3Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
thrustMode = settings.Stabilization4Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
thrustMode = settings.Stabilization5Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
thrustMode = settings.Stabilization6Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
// we hard code the "GPS Assisted" PostionHold to use alt-vario which
// is a more appropriate throttle mode. "GPSAssist" adds braking
// and a better throttle management to the standard Position Hold.
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
}
stabDesired.StabilizationMode.Thrust = thrustMode;
stabDesired.Thrust = manualControl.Thrust;
} else if (manual_thrust) {
stabDesired.Thrust = manualControl.Thrust;
}
// else thrust is set by the PID controller
StabilizationDesiredSet(&stabDesired);
return result;
}
/**
* Compute desired attitude for vtols - emergency fallback
*/
static void updateVtolDesiredAttitudeEmergencyFallback()
{
const float dT = updatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
float courseError;
float courseCommand;
float downError;
float downCommand;
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
courseCommand = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP);
stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max);
// Compute desired down command
downError = velocityDesired.Down - velocityState.Down;
// Must flip this sign
downError = -downError;
downCommand = pid_apply(&global.PIDvel[2], downError, dT);
stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
stabDesired.Roll = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll;
stabDesired.Pitch = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch;
if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
// For now override thrust with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Thrust = manualControl.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
StabilizationDesiredSet(&stabDesired);
}
/**
* Compute desired attitude from a fixed preset
*
*/
static void updateFixedAttitude(float *attitude)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
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);
}

462
flight/modules/PathFollower/pathfollower.cpp Normal file
View File

@ -0,0 +1,462 @@
/**
******************************************************************************
*
* @file pathfollower.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: PathDesired
* Input object: PositionState
* Input object: ManualControlCommand
* Output object: StabilizationDesired
*
* This module acts as "autopilot" - it controls the setpoints of stabilization
* based on current flight situation and desired flight path (PathDesired) as
* directed by flightmode selection or pathplanner
* This is a periodic delayed callback module
*
* 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
*
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <groundpathfollowersettings.h>
#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
#include <pidstatus.h>
#include <homelocation.h>
#include <accelstate.h>
#include <statusvtolland.h>
#include <statusgrounddrive.h>
}
#include "pathfollowercontrol.h"
#include "vtollandcontroller.h"
#include "vtolvelocitycontroller.h"
#include "vtolbrakecontroller.h"
#include "vtolflycontroller.h"
#include "fixedwingflycontroller.h"
#include "grounddrivecontroller.h"
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define PF_IDLE_UPDATE_RATE_MS 100
#define STACK_SIZE_BYTES 2048
// Private types
// Private variables
static DelayedCallbackInfo *pathFollowerCBInfo;
static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
static PathStatusData pathStatus;
static PathDesiredData pathDesired;
static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
static GroundPathFollowerSettingsData groundPathFollowerSettings;
static VtolPathFollowerSettingsData vtolPathFollowerSettings;
static FlightStatusData flightStatus;
static PathFollowerControl *activeController = 0;
// Private functions
static void pathFollowerTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
static void pathFollowerObjectiveUpdatedCb(UAVObjEvent *ev);
static void flightStatusUpdatedCb(UAVObjEvent *ev);
static void updateFixedAttitude(float *attitude);
static void pathFollowerInitializeControllersForFrameType();
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
extern "C" int32_t PathFollowerStart()
{
// Start main task
PathStatusGet(&pathStatus);
SettingsUpdatedCb(NULL);
PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
extern "C" int32_t PathFollowerInitialize()
{
// initialize objects
GroundPathFollowerSettingsInitialize();
FixedWingPathFollowerSettingsInitialize();
FixedWingPathFollowerStatusInitialize();
VtolPathFollowerSettingsInitialize();
FlightStatusInitialize();
FlightModeSettingsInitialize();
PathStatusInitialize();
PathSummaryInitialize();
PathDesiredInitialize();
PositionStateInitialize();
VelocityStateInitialize();
VelocityDesiredInitialize();
StabilizationDesiredInitialize();
AirspeedStateInitialize();
AttitudeStateInitialize();
TakeOffLocationInitialize();
PoiLocationInitialize();
ManualControlCommandInitialize();
SystemSettingsInitialize();
StabilizationBankInitialize();
VtolSelfTuningStatsInitialize();
PIDStatusInitialize();
StatusVtolLandInitialize();
StatusGroundDriveInitialize();
// VtolLandFSM additional objects
HomeLocationInitialize();
AccelStateInitialize();
// Init references to controllers
PathFollowerControl::Initialize(&pathDesired, &flightStatus, &pathStatus);
// Create object queue
pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
GroundPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
PathDesiredConnectCallback(&pathFollowerObjectiveUpdatedCb);
FlightStatusConnectCallback(&flightStatusUpdatedCb);
SystemSettingsConnectCallback(&SettingsUpdatedCb);
AirspeedStateConnectCallback(&airspeedStateUpdatedCb);
VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
return 0;
}
MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
void pathFollowerInitializeControllersForFrameType()
{
static uint8_t multirotor_initialised = 0;
static uint8_t fixedwing_initialised = 0;
static uint8_t ground_initialised = 0;
switch (frameType) {
case FRAME_TYPE_MULTIROTOR:
case FRAME_TYPE_HELI:
if (!multirotor_initialised) {
VtolLandController::instance()->Initialize(&vtolPathFollowerSettings);
VtolVelocityController::instance()->Initialize(&vtolPathFollowerSettings);
VtolFlyController::instance()->Initialize(&vtolPathFollowerSettings);
VtolBrakeController::instance()->Initialize(&vtolPathFollowerSettings);
multirotor_initialised = 1;
}
break;
case FRAME_TYPE_FIXED_WING:
if (!fixedwing_initialised) {
FixedWingFlyController::instance()->Initialize(&fixedWingPathFollowerSettings);
fixedwing_initialised = 1;
}
break;
case FRAME_TYPE_GROUND:
if (!ground_initialised) {
GroundDriveController::instance()->Initialize(&groundPathFollowerSettings);
ground_initialised = 1;
}
break;
default:
break;
}
}
static void pathFollowerSetActiveController(void)
{
// Init controllers for the frame type
if (activeController == 0) {
// Initialise
pathFollowerInitializeControllersForFrameType();
switch (frameType) {
case FRAME_TYPE_MULTIROTOR:
case FRAME_TYPE_HELI:
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_BRAKE: // brake then hold sequence controller
activeController = VtolBrakeController::instance();
activeController->Activate();
break;
case PATHDESIRED_MODE_VELOCITY: // velocity roam controller
activeController = VtolVelocityController::instance();
activeController->Activate();
break;
case PATHDESIRED_MODE_GOTOENDPOINT:
case PATHDESIRED_MODE_FOLLOWVECTOR:
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
activeController = VtolFlyController::instance();
activeController->Activate();
break;
case PATHDESIRED_MODE_LAND: // land with optional velocity roam option
activeController = VtolLandController::instance();
activeController->Activate();
break;
default:
activeController = 0;
break;
}
break;
case FRAME_TYPE_FIXED_WING:
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_GOTOENDPOINT:
case PATHDESIRED_MODE_FOLLOWVECTOR:
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
activeController = FixedWingFlyController::instance();
activeController->Activate();
break;
default:
activeController = 0;
break;
}
break;
case FRAME_TYPE_GROUND:
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_GOTOENDPOINT:
case PATHDESIRED_MODE_FOLLOWVECTOR:
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
activeController = GroundDriveController::instance();
activeController->Activate();
break;
default:
activeController = 0;
break;
}
break;
default:
activeController = 0;
break;
}
}
}
/**
* Module thread, should not return.
*/
static void pathFollowerTask(void)
{
FlightStatusGet(&flightStatus);
if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
}
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
pathFollowerSetActiveController();
if (activeController) {
activeController->UpdateAutoPilot();
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
return;
}
switch (pathDesired.Mode) {
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);
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
}
static void pathFollowerObjectiveUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PathDesiredGet(&pathDesired);
if (activeController && pathDesired.Mode != activeController->Mode()) {
activeController->Deactivate();
activeController = 0;
}
pathFollowerSetActiveController();
if (activeController) {
activeController->ObjectiveUpdated();
}
}
// we use this to deactivate active controllers as the pathdesired
// objective never gets updated with switching to a non-pathfollower flight mode
static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
if (!activeController) {
return;
}
FlightStatusControlChainData controlChain;
FlightStatusControlChainGet(&controlChain);
if (controlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
activeController->Deactivate();
activeController = 0;
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
FrameType_t previous_frameType = frameType;
frameType = GetCurrentFrameType();
if (frameType == FRAME_TYPE_CUSTOM) {
switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
break;
}
}
// If frame type changes, initialise the frame type controllers
if (frameType != previous_frameType) {
pathFollowerInitializeControllersForFrameType();
}
switch (frameType) {
case FRAME_TYPE_MULTIROTOR:
case FRAME_TYPE_HELI:
updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
break;
case FRAME_TYPE_FIXED_WING:
FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
break;
case FRAME_TYPE_GROUND:
GroundPathFollowerSettingsGet(&groundPathFollowerSettings);
updatePeriod = groundPathFollowerSettings.UpdatePeriod;
break;
default:
updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
break;
}
if (activeController) {
activeController->SettingsUpdated();
}
}
static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingFlyController::instance()->AirspeedStateUpdatedCb(ev);
}
/**
* Compute desired attitude from a fixed preset
*
*/
static void updateFixedAttitude(float *attitude)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
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);
}

52
flight/modules/PathFollower/pathfollowercontrol.cpp Normal file
View File

@ -0,0 +1,52 @@
/*
******************************************************************************
*
* @file PathFollowerControl.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Controller interface class implementation
* @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
*/
extern "C" {
#include <openpilot.h>
#include <flightstatus.h>
#include <pathstatus.h>
#include <pathdesired.h>
}
// C++ includes
#include "pathfollowercontrol.h"
PathDesiredData *PathFollowerControl::pathDesired = 0;
FlightStatusData *PathFollowerControl::flightStatus = 0;
PathStatusData *PathFollowerControl::pathStatus = 0;
int32_t PathFollowerControl::Initialize(PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus,
PathStatusData *ptr_pathStatus)
{
PIOS_Assert(ptr_pathDesired);
PIOS_Assert(ptr_flightStatus);
PIOS_Assert(ptr_pathStatus);
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
pathStatus = ptr_pathStatus;
return 0;
}

375
flight/modules/PathFollower/pidcontroldown.cpp Normal file
View File

@ -0,0 +1,375 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower PID interface class
* @brief PID loop for down control
* @{
*
* @file pidcontroldown.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes PID control for down direction
*
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <stabilizationdesired.h>
#include <pidstatus.h>
#include <vtolselftuningstats.h>
}
#include "pathfollowerfsm.h"
#include "pidcontroldown.h"
#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT 0.2f
#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT 0.2f
#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT 0.1f
#define NEUTRALTHRUST_START_DELAY (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate)
#define NEUTRALTHRUST_END_COUNT (NEUTRALTHRUST_START_DELAY + (4 * 20)) // 4 second sample
PIDControlDown::PIDControlDown()
: deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f),
mFSM(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false)
{
Deactivate();
}
PIDControlDown::~PIDControlDown() {}
void PIDControlDown::Initialize(PathFollowerFSM *fsm)
{
mFSM = fsm;
}
void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
{
mMinThrust = min_thrust;
mMaxThrust = max_thrust;
}
void PIDControlDown::Deactivate()
{
// pid_zero(&PID);
mActive = false;
}
void PIDControlDown::Activate()
{
float currentThrust;
StabilizationDesiredThrustGet(&currentThrust);
float u0 = currentThrust - mNeutral;
pid2_transfer(&PID, u0);
mActive = true;
}
void PIDControlDown::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
{
// pid_configure(&PID, kp, ki, kd, ilimit);
float Ti = kp / ki;
float Td = kd / kp;
float Tt = (Ti + Td) / 2.0f;
float kt = 1.0f / Tt;
float N = 10.0f;
float Tf = Td / N;
if (ki < 1e-6f) {
// Avoid Ti being infinite
Ti = 1e6f;
// Tt antiwindup time constant - we don't need antiwindup with no I term
Tt = 1e6f;
kt = 0.0f;
}
if (kd < 1e-6f) {
// PI Controller or P Controller
Tf = Ti / N;
}
if (beta > 1.0f) {
beta = 1.0f;
} else if (beta < 0.4f) {
beta = 0.4f;
}
pid2_configure(&PID, kp, ki, kd, Tf, kt, dT, beta, mNeutral, mNeutral, -1.0f);
deltaTime = dT;
mVelocityMax = velocityMax;
}
void PIDControlDown::UpdatePositionalParameters(float kp)
{
pid_configure(&PIDpos, kp, 0.0f, 0.0f, 0.0f);
}
void PIDControlDown::UpdatePositionSetpoint(float setpointDown)
{
mPositionSetpointTarget = setpointDown;
}
void PIDControlDown::UpdatePositionState(float pvDown)
{
mPositionState = pvDown;
setup_neutralThrustCalc();
}
// This is a pure position hold position control
void PIDControlDown::ControlPosition()
{
// Current progress location relative to end
float velDown = 0.0f;
velDown = pid_apply(&PIDpos, mPositionSetpointTarget - mPositionState, deltaTime);
UpdateVelocitySetpoint(velDown);
run_neutralThrustCalc();
}
void PIDControlDown::ControlPositionWithPath(struct path_status *progress)
{
// Current progress location relative to end
float velDown = progress->path_vector[2];
velDown += pid_apply(&PIDpos, progress->correction_vector[2], deltaTime);
UpdateVelocitySetpoint(velDown);
}
void PIDControlDown::run_neutralThrustCalc(void)
{
// if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment
// note that arming into this flight mode is not allowed, so assumption here is that
// we have not landed. If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the
// altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL.
// In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min,
// avoiding auto-takeoffs. Therefore no need to check that here.
if (neutralThrustEst.have_correction != true) {
// Make FSM specific
bool stable = (fabsf(mPositionSetpointTarget - mPositionState) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT &&
fabsf(mVelocitySetpointCurrent) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT &&
fabsf(mVelocityState) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT &&
fabsf(mVelocitySetpointCurrent - mVelocityState) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT);
if (stable) {
if (neutralThrustEst.start_sampling) {
neutralThrustEst.count++;
// delay count for 2 seconds into hold allowing for stablisation
if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) {
neutralThrustEst.sum += PID.I;
if (PID.I < neutralThrustEst.min) {
neutralThrustEst.min = PID.I;
}
if (PID.I > neutralThrustEst.max) {
neutralThrustEst.max = PID.I;
}
}
if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) {
// 6 seconds have past
// lets take an average
neutralThrustEst.average = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY);
neutralThrustEst.correction = neutralThrustEst.average;
PID.I -= neutralThrustEst.average;
neutralThrustEst.start_sampling = false;
neutralThrustEst.have_correction = true;
// Write a new adjustment value
// vtolSelfTuningStats.NeutralThrustOffset was incremental adjusted above
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
// add the average remaining i value to the
vtolSelfTuningStats.NeutralThrustOffset += neutralThrustEst.correction;
vtolSelfTuningStats.NeutralThrustCorrection = neutralThrustEst.correction; // the i term thrust correction value applied
vtolSelfTuningStats.NeutralThrustAccumulator = PID.I; // the actual iaccumulator value after correction
vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
VtolSelfTuningStatsSet(&vtolSelfTuningStats);
}
} else {
// start a tick count
neutralThrustEst.start_sampling = true;
neutralThrustEst.count = 0;
neutralThrustEst.sum = 0.0f;
neutralThrustEst.max = 0.0f;
neutralThrustEst.min = 0.0f;
}
} else {
// reset sampling as we did't get 6 continuous seconds
neutralThrustEst.start_sampling = false;
}
} // else we already have a correction for this PH run
}
void PIDControlDown::setup_neutralThrustCalc(void)
{
// reset neutral thrust assessment.
// and do once for each position hold engagement
neutralThrustEst.start_sampling = false;
neutralThrustEst.count = 0;
neutralThrustEst.sum = 0.0f;
neutralThrustEst.have_correction = false;
neutralThrustEst.average = 0.0f;
neutralThrustEst.correction = 0.0f;
neutralThrustEst.min = 0.0f;
neutralThrustEst.max = 0.0f;
}
void PIDControlDown::UpdateNeutralThrust(float neutral)
{
if (mActive) {
// adjust neutral and achieve bumpless transfer
PID.va = neutral;
pid2_transfer(&PID, mDownCommand);
}
mNeutral = neutral;
}
void PIDControlDown::UpdateVelocitySetpoint(float setpoint)
{
mVelocitySetpointTarget = setpoint;
if (fabsf(mVelocitySetpointTarget) > mVelocityMax) {
// maintain sign but set to max
mVelocitySetpointTarget *= mVelocityMax / fabsf(mVelocitySetpointTarget);
}
}
void PIDControlDown::RateLimit(float *spDesired, float *spCurrent, float rateLimit)
{
float velocity_delta = *spDesired - *spCurrent;
if (fabsf(velocity_delta) < 1e-6f) {
*spCurrent = *spDesired;
return;
}
// Calculate the rate of change
float accelerationDesired = velocity_delta / deltaTime;
if (fabsf(accelerationDesired) > rateLimit) {
accelerationDesired *= rateLimit / accelerationDesired;
}
if (fabsf(accelerationDesired) < 0.1f) {
*spCurrent = *spDesired;
} else {
*spCurrent += accelerationDesired * deltaTime;
}
}
void PIDControlDown::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
{
if (startingVelocity >= 0.0f) {
*updatedVelocity = startingVelocity - dT * brakeRate;
if (*updatedVelocity > currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity < 0.0f) {
*updatedVelocity = 0.0f;
}
} else {
*updatedVelocity = startingVelocity + dT * brakeRate;
if (*updatedVelocity < currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity > 0.0f) {
*updatedVelocity = 0.0f;
}
}
}
void PIDControlDown::UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate)
{
mVelocityState = pvDown;
float velocitySetpointDesired;
UpdateBrakeVelocity(mVelocitySetpointTarget, path_time, brakeRate, pvDown, &velocitySetpointDesired);
// Calculate the rate of change
// RateLimit(velocitySetpointDesired[iaxis], mVelocitySetpointCurrent[iaxis], 2.0f );
mVelocitySetpointCurrent = velocitySetpointDesired;
}
// Update velocity state called per dT. Also update current
// desired velocity
void PIDControlDown::UpdateVelocityState(float pv)
{
mVelocityState = pv;
if (mFSM) {
// The FSM controls the actual descent velocity and introduces step changes as required
float velocitySetpointDesired = mFSM->BoundVelocityDown(mVelocitySetpointTarget);
// RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f );
mVelocitySetpointCurrent = velocitySetpointDesired;
} else {
mVelocitySetpointCurrent = mVelocitySetpointTarget;
}
}
float PIDControlDown::GetVelocityDesired(void)
{
return mVelocitySetpointCurrent;
}
float PIDControlDown::GetDownCommand(void)
{
PIDStatusData pidStatus;
float ulow = mMinThrust;
float uhigh = mMaxThrust;
if (mFSM) {
mFSM->BoundThrust(ulow, uhigh);
}
float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh);
pidStatus.setpoint = mVelocitySetpointCurrent;
pidStatus.actual = mVelocityState;
pidStatus.error = mVelocitySetpointCurrent - mVelocityState;
pidStatus.setpoint = mVelocitySetpointCurrent;
pidStatus.ulow = ulow;
pidStatus.uhigh = uhigh;
pidStatus.command = downCommand;
pidStatus.P = PID.P;
pidStatus.I = PID.I;
pidStatus.D = PID.D;
PIDStatusSet(&pidStatus);
mDownCommand = downCommand;
return mDownCommand;
}

249
flight/modules/PathFollower/pidcontrolne.cpp Normal file
View File

@ -0,0 +1,249 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief PID controller for NE
* @{
*
* @file PIDControlNE.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes PID control loops for NE directions
*
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <pidstatus.h>
}
#include "pathfollowerfsm.h"
#include "pidcontrolne.h"
PIDControlNE::PIDControlNE()
: deltaTime(0), mNECommand(0), mNeutral(0), mVelocityMax(0), mMinCommand(0), mMaxCommand(0), mVelocityFeedforward(0), mActive(false)
{}
PIDControlNE::~PIDControlNE() {}
void PIDControlNE::Initialize()
{}
void PIDControlNE::Deactivate()
{
mActive = false;
}
void PIDControlNE::Activate()
{
// Do we need to initialise any loops for smooth transition
// float currentNE;
// StabilizationDesiredNEGet(&currentNE);
// float u0 = currentNE - mNeutral;
// pid2_transfer(&PID, u0);
mActive = true;
}
void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
{
// pid_configure(&PID, kp, ki, kd, ilimit);
float Ti = kp / ki;
float Td = kd / kp;
float Tt = (Ti + Td) / 2.0f;
float kt = 1.0f / Tt;
float u0 = 0.0f;
float N = 10.0f;
float Tf = Td / N;
if (ki < 1e-6f) {
// Avoid Ti being infinite
Ti = 1e6f;
// Tt antiwindup time constant - we don't need antiwindup with no I term
Tt = 1e6f;
kt = 0.0f;
}
if (kd < 1e-6f) {
// PI Controller
Tf = Ti / N;
}
if (beta > 1.0f) {
beta = 1.0f;
} else if (beta < 0.4f) {
beta = 0.4f;
}
pid2_configure(&PIDvel[0], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
pid2_configure(&PIDvel[1], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
deltaTime = dT;
mVelocityMax = velocityMax;
}
void PIDControlNE::UpdatePositionalParameters(float kp)
{
pid_configure(&PIDposH[0], kp, 0.0f, 0.0f, 0.0f);
pid_configure(&PIDposH[1], kp, 0.0f, 0.0f, 0.0f);
}
void PIDControlNE::UpdatePositionSetpoint(float setpointNorth, float setpointEast)
{
mPositionSetpointTarget[0] = setpointNorth;
mPositionSetpointTarget[1] = setpointEast;
}
void PIDControlNE::UpdatePositionState(float pvNorth, float pvEast)
{
mPositionState[0] = pvNorth;
mPositionState[1] = pvEast;
}
// This is a pure position hold position control
void PIDControlNE::ControlPosition()
{
// Current progress location relative to end
float velNorth = 0.0f;
float velEast = 0.0f;
velNorth = pid_apply(&PIDposH[0], mPositionSetpointTarget[0] - mPositionState[0], deltaTime);
velEast = pid_apply(&PIDposH[1], mPositionSetpointTarget[1] - mPositionState[1], deltaTime);
UpdateVelocitySetpoint(velNorth, velEast);
}
void PIDControlNE::ControlPositionWithPath(struct path_status *progress)
{
// Current progress location relative to end
float velNorth = progress->path_vector[0];
float velEast = progress->path_vector[1];
velNorth += pid_apply(&PIDposH[0], progress->correction_vector[0], deltaTime);
velEast += pid_apply(&PIDposH[1], progress->correction_vector[1], deltaTime);
UpdateVelocitySetpoint(velNorth, velEast);
}
void PIDControlNE::UpdateVelocitySetpoint(float setpointNorth, float setpointEast)
{
// scale velocity if it is above configured maximum
// for braking, we can not help it if initial velocity was greater
float velH = sqrtf(setpointNorth * setpointNorth + setpointEast * setpointEast);
if (velH > mVelocityMax) {
setpointNorth *= mVelocityMax / velH;
setpointEast *= mVelocityMax / velH;
}
mVelocitySetpointTarget[0] = setpointNorth;
mVelocitySetpointTarget[1] = setpointEast;
}
void PIDControlNE::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
{
if (startingVelocity >= 0.0f) {
*updatedVelocity = startingVelocity - dT * brakeRate;
if (*updatedVelocity > currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity < 0.0f) {
*updatedVelocity = 0.0f;
}
} else {
*updatedVelocity = startingVelocity + dT * brakeRate;
if (*updatedVelocity < currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity > 0.0f) {
*updatedVelocity = 0.0f;
}
}
}
void PIDControlNE::UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate)
{
mVelocityState[0] = pvNorth;
mVelocityState[1] = pvEast;
float velocitySetpointDesired[2];
UpdateBrakeVelocity(mVelocitySetpointTarget[0], path_time, brakeRate, pvNorth, &velocitySetpointDesired[0]);
UpdateBrakeVelocity(mVelocitySetpointTarget[1], path_time, brakeRate, pvEast, &velocitySetpointDesired[1]);
// If rate of change limits required, add here
for (int iaxis = 0; iaxis < 2; iaxis++) {
mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
}
}
void PIDControlNE::UpdateVelocityState(float pvNorth, float pvEast)
{
mVelocityState[0] = pvNorth;
mVelocityState[1] = pvEast;
// The FSM controls the actual descent velocity and introduces step changes as required
float velocitySetpointDesired[2];
velocitySetpointDesired[0] = mVelocitySetpointTarget[0];
velocitySetpointDesired[1] = mVelocitySetpointTarget[1];
// If rate of change limits required, add here
for (int iaxis = 0; iaxis < 2; iaxis++) {
mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
}
}
void PIDControlNE::UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward)
{
mMinCommand = minCommand;
mMaxCommand = maxCommand;
mVelocityFeedforward = velocityFeedforward;
}
void PIDControlNE::GetNECommand(float *northCommand, float *eastCommand)
{
PIDvel[0].va = mVelocitySetpointCurrent[0] * mVelocityFeedforward;
*northCommand = pid2_apply(&(PIDvel[0]), mVelocitySetpointCurrent[0], mVelocityState[0], mMinCommand, mMaxCommand);
PIDvel[1].va = mVelocitySetpointCurrent[1] * mVelocityFeedforward;
*eastCommand = pid2_apply(&(PIDvel[1]), mVelocitySetpointCurrent[1], mVelocityState[1], mMinCommand, mMaxCommand);
PIDStatusData pidStatus;
pidStatus.setpoint = mVelocitySetpointCurrent[0];
pidStatus.actual = mVelocityState[0];
pidStatus.error = mVelocitySetpointCurrent[0] - mVelocityState[0];
pidStatus.setpoint = mVelocitySetpointCurrent[0];
pidStatus.ulow = mMinCommand;
pidStatus.uhigh = mMaxCommand;
pidStatus.command = *northCommand;
pidStatus.P = PIDvel[0].P;
pidStatus.I = PIDvel[0].I;
pidStatus.D = PIDvel[0].D;
PIDStatusSet(&pidStatus);
}
void PIDControlNE::GetVelocityDesired(float *north, float *east)
{
*north = mVelocitySetpointCurrent[0];
*east = mVelocitySetpointCurrent[1];
}

355
flight/modules/PathFollower/vtolbrakecontroller.cpp Normal file
View File

@ -0,0 +1,355 @@
/*
******************************************************************************
*
* @file vtolbrakecontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This landing state machine is a helper state machine to the
* pathfollower task/thread to implement detailed landing controls.
* This is to be called only from the pathfollower task.
* Note that initiation of the land occurs in the manual control
* command thread calling plans.c plan_setup_land which writes
* the required PathDesired LAND 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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtolbrakecontroller.h"
#include "pathfollowerfsm.h"
#include "vtolbrakefsm.h"
#include "pidcontroldown.h"
// Private constants
#define BRAKE_RATE_MINIMUM 0.2f
// pointer to a singleton instance
VtolBrakeController *VtolBrakeController::p_inst = 0;
VtolBrakeController::VtolBrakeController()
: fsm(0), vtolPathFollowerSettings(0), mActive(false), mHoldActive(false), mManualThrust(false)
{}
// Called when mode first engaged
void VtolBrakeController::Activate(void)
{
if (!mActive) {
mActive = true;
mHoldActive = false;
mManualThrust = false;
SettingsUpdated();
fsm->Activate();
controlDown.Activate();
controlNE.Activate();
}
}
uint8_t VtolBrakeController::IsActive(void)
{
return mActive;
}
uint8_t VtolBrakeController::Mode(void)
{
return PATHDESIRED_MODE_BRAKE;
}
// Objective updated in pathdesired
void VtolBrakeController::ObjectiveUpdated(void)
{
// Set the objective's target velocity
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN]);
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH],
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST]);
if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
pathStatus->path_time = 0.0f;
}
}
void VtolBrakeController::Deactivate(void)
{
if (mActive) {
mActive = false;
mHoldActive = false;
mManualThrust = false;
fsm->Inactive();
controlDown.Deactivate();
controlNE.Deactivate();
}
}
void VtolBrakeController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp,
vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki,
vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd,
vtolPathFollowerSettings->BrakeHorizontalVelPID.ILimit,
dT,
10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
vtolPathFollowerSettings->LandVerticalVelPID.Ki,
vtolPathFollowerSettings->LandVerticalVelPID.Kd,
vtolPathFollowerSettings->LandVerticalVelPID.Beta,
dT,
10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
fsm->SettingsUpdated();
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolBrakeController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
if (fsm == 0) {
VtolBrakeFSM::instance()->Initialize(vtolPathFollowerSettings, pathDesired, flightStatus, pathStatus);
fsm = (PathFollowerFSM *)VtolBrakeFSM::instance();
controlDown.Initialize(fsm);
}
return 0;
}
void VtolBrakeController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
float brakeRate = vtolPathFollowerSettings->BrakeRate;
if (brakeRate < BRAKE_RATE_MINIMUM) {
brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
}
if (fsm->PositionHoldState()) {
PositionStateData positionState;
PositionStateGet(&positionState);
// On first engagement set the position setpoint
if (!mHoldActive) {
mHoldActive = true;
controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
if (!mManualThrust) {
controlDown.UpdatePositionSetpoint(positionState.Down);
}
}
// Update position state and control position to create inputs to velocity control
controlNE.UpdatePositionState(positionState.North, positionState.East);
controlNE.ControlPosition();
if (!mManualThrust) {
controlDown.UpdatePositionState(positionState.Down);
controlDown.ControlPosition();
}
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
if (!mManualThrust) {
controlDown.UpdateVelocityState(velocityState.Down);
}
} else {
controlNE.UpdateVelocityStateWithBrake(velocityState.North, velocityState.East, pathStatus->path_time, brakeRate);
if (!mManualThrust) {
controlDown.UpdateVelocityStateWithBrake(velocityState.Down, pathStatus->path_time, brakeRate);
}
}
if (!mManualThrust) {
velocityDesired.Down = controlDown.GetVelocityDesired();
} else { velocityDesired.Down = 0.0f; }
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
VelocityDesiredSet(&velocityDesired);
// update pathstatus
float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
cur_velocity = sqrtf(cur_velocity);
float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down;
desired_velocity = sqrtf(desired_velocity);
pathStatus->error = cur_velocity - desired_velocity;
pathStatus->fractional_progress = 1.0f;
if (pathDesired->StartingVelocity > 0.0f) {
pathStatus->fractional_progress = (pathDesired->StartingVelocity - cur_velocity) / pathDesired->StartingVelocity;
// sometimes current velocity can exceed starting velocity due to initial acceleration
if (pathStatus->fractional_progress < 0.0f) {
pathStatus->fractional_progress = 0.0f;
}
}
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
}
int8_t VtolBrakeController::UpdateStabilizationDesired(void)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->BrakeMaxPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
// when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode.
if (flightStatus->FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) {
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
uint8_t thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
switch (flightStatus->FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
thrustMode = settings.Stabilization1Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
thrustMode = settings.Stabilization2Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
thrustMode = settings.Stabilization3Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
thrustMode = settings.Stabilization4Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
thrustMode = settings.Stabilization5Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
thrustMode = settings.Stabilization6Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
// we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
// is a more appropriate throttle mode. "GPSAssist" adds braking
// and a better throttle management to the standard Position Hold.
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
}
stabDesired.StabilizationMode.Thrust = thrustMode;
}
// set the thrust value
if (mManualThrust) {
stabDesired.Thrust = manualControl.Thrust;
} else {
stabDesired.Thrust = controlDown.GetDownCommand();
}
StabilizationDesiredSet(&stabDesired);
return result;
}
void VtolBrakeController::UpdateAutoPilot()
{
if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
pathStatus->path_time += vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
}
if (flightStatus->FlightModeAssist && flightStatus->AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) {
mManualThrust = true;
} else {
mManualThrust = false;
}
fsm->Update(); // This will run above end condition checks
UpdateVelocityDesired();
int8_t result = UpdateStabilizationDesired();
if (!result) {
fsm->Abort(); // enter PH
}
PathStatusSet(pathStatus);
}

262
flight/modules/PathFollower/vtolbrakefsm.cpp Normal file
View File

@ -0,0 +1,262 @@
/*
******************************************************************************
*
* @file vtolbrakefsm.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol brake finate state machine to regulate behaviour of the
* brake controller.
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include <vtolbrakefsm.h>
// Private constants
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f
#define BRAKE_EXIT_VELOCITY_LIMIT 0.2f
VtolBrakeFSM::PathFollowerFSM_BrakeStateHandler_T VtolBrakeFSM::sBrakeStateTable[BRAKE_STATE_SIZE] = {
[BRAKE_STATE_INACTIVE] = { .setup = 0, .run = 0 },
[BRAKE_STATE_BRAKE] = { .setup = &VtolBrakeFSM::setup_brake, .run = &VtolBrakeFSM::run_brake },
[BRAKE_STATE_HOLD] = { .setup = 0, .run = 0 }
};
// pointer to a singleton instance
VtolBrakeFSM *VtolBrakeFSM::p_inst = 0;
VtolBrakeFSM::VtolBrakeFSM()
: mBrakeData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
{}
// Private types
// Private functions
// Public API methods
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolBrakeFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus,
PathStatusData *ptr_pathStatus)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
PIOS_Assert(ptr_pathDesired);
PIOS_Assert(ptr_flightStatus);
// allow for Initialize being called more than once.
if (!mBrakeData) {
mBrakeData = (VtolBrakeFSMData_T *)pios_malloc(sizeof(VtolBrakeFSMData_T));
PIOS_Assert(mBrakeData);
}
memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
pathStatus = ptr_pathStatus;
initFSM();
return 0;
}
void VtolBrakeFSM::Inactive(void)
{
memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0);
initFSM();
}
// Initialise the FSM
void VtolBrakeFSM::initFSM(void)
{
mBrakeData->currentState = BRAKE_STATE_INACTIVE;
}
void VtolBrakeFSM::Activate()
{
memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0);
mBrakeData->currentState = BRAKE_STATE_INACTIVE;
setState(BRAKE_STATE_BRAKE, FSMBRAKESTATUS_STATEEXITREASON_NONE);
}
void VtolBrakeFSM::Abort(void)
{
setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolBrakeFSM::GetCurrentState(void)
{
switch (mBrakeData->currentState) {
case BRAKE_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
default:
return PFFSM_STATE_ACTIVE;
break;
}
}
void VtolBrakeFSM::Update()
{
runState();
}
int32_t VtolBrakeFSM::runState(void)
{
uint8_t flTimeout = false;
mBrakeData->stateRunCount++;
if (mBrakeData->stateTimeoutCount > 0 && mBrakeData->stateRunCount > mBrakeData->stateTimeoutCount) {
flTimeout = true;
}
// If the current state has a static function, call it
if (sBrakeStateTable[mBrakeData->currentState].run) {
(this->*sBrakeStateTable[mBrakeData->currentState].run)(flTimeout);
}
return 0;
}
// Set the new state and perform setup for subsequent state run calls
// This is called by state run functions on event detection that drive
// state transitions.
void VtolBrakeFSM::setState(PathFollowerFSM_BrakeState_T newState, __attribute__((unused)) VtolBrakeFSMStatusStateExitReasonOptions reason)
{
// mBrakeData->fsmBrakeStatus.StateExitReason[mBrakeData->currentState] = reason;
if (mBrakeData->currentState == newState) {
return;
}
mBrakeData->currentState = newState;
// Restart state timer counter
mBrakeData->stateRunCount = 0;
// Reset state timeout to disabled/zero
mBrakeData->stateTimeoutCount = 0;
if (sBrakeStateTable[mBrakeData->currentState].setup) {
(this->*sBrakeStateTable[mBrakeData->currentState].setup)();
}
}
// Timeout utility function for use by state init implementations
void VtolBrakeFSM::setStateTimeout(int32_t count)
{
mBrakeData->stateTimeoutCount = count;
}
// FSM Setup and Run method implementation
// State: WAITING FOR DESCENT RATE
void VtolBrakeFSM::setup_brake(void)
{
setStateTimeout(TIMER_COUNT_PER_SECOND * pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]);
mBrakeData->observationCount = 0;
mBrakeData->observation2Count = 0;
}
void VtolBrakeFSM::run_brake(uint8_t flTimeout)
{
// Brake mode end condition checks
bool exit_brake = false;
VelocityStateData velocityState;
PathSummaryData pathSummary;
if (flTimeout) {
pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT;
exit_brake = true;
} else if (pathStatus->fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) {
VelocityStateGet(&velocityState);
if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) {
pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED;
exit_brake = true;
}
}
if (exit_brake) {
// Calculate the distance error between the originally desired
// stopping point and the actual brake-exit point.
PositionStateData p;
PositionStateGet(&p);
float north_offset = pathDesired->End.North - p.North;
float east_offset = pathDesired->End.East - p.East;
float down_offset = pathDesired->End.Down - p.Down;
pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset);
pathSummary.time_remaining = pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus->path_time;
pathSummary.fractional_progress = pathStatus->fractional_progress;
float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
cur_velocity = sqrtf(cur_velocity);
pathSummary.decelrate = (pathDesired->StartingVelocity - cur_velocity) / pathStatus->path_time;
pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings->BrakeRate;
pathSummary.velocityIntoHold = cur_velocity;
pathSummary.Mode = PATHSUMMARY_MODE_BRAKE;
pathSummary.UID = pathStatus->UID;
PathSummarySet(&pathSummary);
setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE);
}
}
uint8_t VtolBrakeFSM::PositionHoldState(void)
{
return mBrakeData->currentState == BRAKE_STATE_HOLD;
}

547
flight/modules/PathFollower/vtolflycontroller.cpp Normal file
View File

@ -0,0 +1,547 @@
/*
******************************************************************************
*
* @file vtolflycontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Class implements the fly controller for vtols
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtolflycontroller.h"
#include "pathfollowerfsm.h"
#include "pidcontroldown.h"
#include "pidcontrolne.h"
// Private constants
#define DEADBAND_HIGH 0.10f
#define DEADBAND_LOW -0.10f
#define RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS 0.95f
#define RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE 2.0f
// pointer to a singleton instance
VtolFlyController *VtolFlyController::p_inst = 0;
VtolFlyController::VtolFlyController()
: vtolPathFollowerSettings(NULL), mActive(false), mManualThrust(false), mMode(0), vtolEmergencyFallback(0.0f), vtolEmergencyFallbackSwitch(false)
{}
// Called when mode first engaged
void VtolFlyController::Activate(void)
{
if (!mActive) {
mActive = true;
mManualThrust = false;
SettingsUpdated();
controlDown.Activate();
controlNE.Activate();
mMode = pathDesired->Mode;
vtolEmergencyFallback = 0.0f;
vtolEmergencyFallbackSwitch = false;
}
}
uint8_t VtolFlyController::IsActive(void)
{
return mActive;
}
uint8_t VtolFlyController::Mode(void)
{
return mMode;
}
// Objective updated in pathdesired
void VtolFlyController::ObjectiveUpdated(void)
{}
void VtolFlyController::Deactivate(void)
{
if (mActive) {
mActive = false;
mManualThrust = false;
controlDown.Deactivate();
controlNE.Deactivate();
vtolEmergencyFallback = 0.0f;
vtolEmergencyFallbackSwitch = false;
}
}
void VtolFlyController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.ILimit,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
vtolPathFollowerSettings->VerticalVelPID.Ki,
vtolPathFollowerSettings->VerticalVelPID.Kd,
vtolPathFollowerSettings->VerticalVelPID.ILimit, // TODO Change to BETA
dT,
vtolPathFollowerSettings->VerticalVelMax);
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolFlyController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
return 0;
}
/**
* Compute desired velocity from the current position and path
*/
void VtolFlyController::UpdateVelocityDesired()
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
controlDown.UpdateVelocityState(velocityState.Down);
VelocityDesiredData velocityDesired;
// look ahead kFF seconds
float cur[3] = { positionState.North + (velocityState.North * vtolPathFollowerSettings->CourseFeedForward),
positionState.East + (velocityState.East * vtolPathFollowerSettings->CourseFeedForward),
positionState.Down + (velocityState.Down * vtolPathFollowerSettings->CourseFeedForward) };
struct path_status progress;
path_progress(pathDesired, cur, &progress, true);
controlNE.ControlPositionWithPath(&progress);
if (!mManualThrust) {
controlDown.ControlPositionWithPath(&progress);
}
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
if (!mManualThrust) {
velocityDesired.Down = controlDown.GetVelocityDesired();
} else { velocityDesired.Down = 0.0f; }
// update pathstatus
pathStatus->error = progress.error;
pathStatus->fractional_progress = progress.fractional_progress;
pathStatus->path_direction_north = progress.path_vector[0];
pathStatus->path_direction_east = progress.path_vector[1];
pathStatus->path_direction_down = progress.path_vector[2];
pathStatus->correction_direction_north = progress.correction_vector[0];
pathStatus->correction_direction_east = progress.correction_vector[1];
pathStatus->correction_direction_down = progress.correction_vector[2];
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
// TODO The below need to be rewritten because the PID implementation has changed.
#if 0
// DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
attitudeState.Yaw += 120.0f;
if (attitudeState.Yaw > 180.0f) {
attitudeState.Yaw -= 360.0f;
}
}
if ( // emergency flyaway detection
( // integral already at its limit
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
) &&
// angle between desired and actual velocity >90 degrees (by dot product)
(velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
// quad is moving at significant speed (during flyaway it would keep speeding up)
squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
) {
vtolEmergencyFallback += dT;
if (vtolEmergencyFallback >= vtolPathFollowerSettings->FlyawayEmergencyFallbackTriggerTime) {
// after emergency timeout, trigger alarm - everything else is handled by callers
// (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
result = 0;
}
} else {
vtolEmergencyFallback = 0.0f;
}
#endif // if 0
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
if (mManualThrust) {
stabDesired.Thrust = manualControl.Thrust;
} else {
stabDesired.Thrust = controlDown.GetDownCommand();
}
StabilizationDesiredSet(&stabDesired);
return result;
}
/**
* Compute desired attitude for vtols - emergency fallback
*/
void VtolFlyController::UpdateDesiredAttitudeEmergencyFallback()
{
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
float courseError;
float courseCommand;
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
courseCommand = (courseError * vtolPathFollowerSettings->EmergencyFallbackYawRate.kP);
stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings->EmergencyFallbackYawRate.Max, vtolPathFollowerSettings->EmergencyFallbackYawRate.Max);
controlDown.UpdateVelocitySetpoint(velocityDesired.Down);
controlDown.UpdateVelocityState(velocityState.Down);
stabDesired.Thrust = controlDown.GetDownCommand();
stabDesired.Roll = vtolPathFollowerSettings->EmergencyFallbackAttitude.Roll;
stabDesired.Pitch = vtolPathFollowerSettings->EmergencyFallbackAttitude.Pitch;
if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
stabDesired.Thrust = manualControlData.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
StabilizationDesiredSet(&stabDesired);
}
void VtolFlyController::UpdateAutoPilot()
{
if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
mManualThrust = true;
}
uint8_t result = RunAutoPilot();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(pathStatus);
// If rtbl, detect arrival at the endpoint and then triggers a change
// to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c
// can't manage this. And pathplanner whilst similar does not manage this as it is not a
// waypoint traversal and is not aware of flight modes other than path plan.
if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) {
if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) {
if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) {
plan_setup_land();
}
}
}
}
/**
* vtol autopilot
* use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
* fall back to emergency fallback autopilot to keep minimum amount of flight control
*/
uint8_t VtolFlyController::RunAutoPilot()
{
enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
uint8_t result = 0;
// decide on behaviour based on settings and system state
if (vtolEmergencyFallbackSwitch) {
returnmode = RETURN_0;
followermode = FOLLOWER_FALLBACK;
} else {
if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
returnmode = RETURN_1;
followermode = FOLLOWER_FALLBACK;
} else {
returnmode = RETURN_RESULT;
followermode = FOLLOWER_REGULAR;
}
}
switch (followermode) {
case FOLLOWER_REGULAR:
{
// horizontal position control PID loop works according to settings in regular mode, allowing integral terms
UpdateVelocityDesired();
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = true;
float yaw = 0.0f;
switch (vtolPathFollowerSettings->YawControl) {
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
yaw_attitude = false;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
yaw = updateTailInBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
yaw = updateCourseBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
yaw = updatePathBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
yaw = updatePOIBearing();
break;
}
result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
if (vtolPathFollowerSettings->FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
// switch to emergency follower if follower indicates problems
vtolEmergencyFallbackSwitch = true;
}
}
}
break;
case FOLLOWER_FALLBACK:
{
// fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
controlNE.UpdatePositionalParameters(1.0f);
UpdateVelocityDesired();
// emergency follower has no return value
UpdateDesiredAttitudeEmergencyFallback();
}
break;
}
switch (returnmode) {
case RETURN_RESULT:
return result;
default:
// returns either 0 or 1 according to enum definition above
return returnmode;
}
}
/**
* Compute bearing of current takeoff location
*/
float VtolFlyController::updateTailInBearing()
{
PositionStateData p;
PositionStateGet(&p);
TakeOffLocationData t;
TakeOffLocationGet(&t);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
}
/**
* Compute bearing of current movement direction
*/
float VtolFlyController::updateCourseBearing()
{
VelocityStateData v;
VelocityStateGet(&v);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(v.East, v.North));
}
/**
* Compute bearing of current path direction
*/
float VtolFlyController::updatePathBearing()
{
PositionStateData positionState;
PositionStateGet(&positionState);
float cur[3] = { positionState.North,
positionState.East,
positionState.Down };
struct path_status progress;
path_progress(pathDesired, cur, &progress, true);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
}
/**
* Compute bearing between current position and POI
*/
float VtolFlyController::updatePOIBearing()
{
PoiLocationData poi;
PoiLocationGet(&poi);
PositionStateData positionState;
PositionStateGet(&positionState);
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
float dLoc[3];
float yaw = 0;
/*float elevation = 0;*/
dLoc[0] = positionState.North - poi.North;
dLoc[1] = positionState.East - poi.East;
dLoc[2] = positionState.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
}
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
}
return yaw + (pathAngle / 2.0f);
}

283
flight/modules/PathFollower/vtollandcontroller.cpp Normal file
View File

@ -0,0 +1,283 @@
/*
******************************************************************************
*
* @file vtollandcontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol landing controller loop
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtollandcontroller.h"
#include "pathfollowerfsm.h"
#include "vtollandfsm.h"
#include "pidcontroldown.h"
// Private constants
// pointer to a singleton instance
VtolLandController *VtolLandController::p_inst = 0;
VtolLandController::VtolLandController()
: fsm(NULL), vtolPathFollowerSettings(NULL), mActive(false)
{}
// Called when mode first engaged
void VtolLandController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
fsm->Activate();
controlDown.Activate();
controlNE.Activate();
}
}
uint8_t VtolLandController::IsActive(void)
{
return mActive;
}
uint8_t VtolLandController::Mode(void)
{
return PATHDESIRED_MODE_LAND;
}
// Objective updated in pathdesired, e.g. same flight mode but new target velocity
void VtolLandController::ObjectiveUpdated(void)
{
// Set the objective's target velocity
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
}
void VtolLandController::Deactivate(void)
{
if (mActive) {
mActive = false;
fsm->Inactive();
controlDown.Deactivate();
controlNE.Deactivate();
}
}
void VtolLandController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.ILimit,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
vtolPathFollowerSettings->LandVerticalVelPID.Ki,
vtolPathFollowerSettings->LandVerticalVelPID.Kd,
vtolPathFollowerSettings->LandVerticalVelPID.Beta,
dT,
vtolPathFollowerSettings->VerticalVelMax);
// The following is not currently used in the landing control.
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
// initialise limits on thrust but note the FSM can override.
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
fsm->SettingsUpdated();
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolLandController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
if (fsm == 0) {
fsm = (PathFollowerFSM *)VtolLandFSM::instance();
VtolLandFSM::instance()->Initialize(ptr_vtolPathFollowerSettings, pathDesired, flightStatus);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
controlDown.Initialize(fsm);
}
return 0;
}
void VtolLandController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
controlDown.UpdateVelocityState(velocityState.Down);
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
// Implement optional horizontal position hold.
if (((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) {
// landing flight mode has stored original horizontal position in pathdesired
PositionStateData positionState;
PositionStateGet(&positionState);
controlNE.UpdatePositionState(positionState.North, positionState.East);
controlNE.ControlPosition();
}
velocityDesired.Down = controlDown.GetVelocityDesired();
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
// update pathstatus
pathStatus->error = 0.0f;
pathStatus->fractional_progress = 0.0f;
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus->correction_direction_down = velocityDesired.Down - velocityState.Down;
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolLandController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
stabDesired.Thrust = controlDown.GetDownCommand();
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
StabilizationDesiredSet(&stabDesired);
return result;
}
void VtolLandController::UpdateAutoPilot()
{
fsm->Update();
UpdateVelocityDesired();
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = false;
float yaw = 0.0f;
fsm->GetYaw(yaw_attitude, yaw);
int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
fsm->Abort();
}
if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
}
PathStatusSet(pathStatus);
}
void VtolLandController::setArmedIfChanged(uint8_t val)
{
if (flightStatus->Armed != val) {
flightStatus->Armed = val;
FlightStatusSet(flightStatus);
}
}

703
flight/modules/PathFollower/vtollandfsm.cpp Normal file
View File

@ -0,0 +1,703 @@
/*
******************************************************************************
*
* @file vtollandfsm.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This landing state machine is a helper state machine to the
* VtolLandController.
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <statusvtolland.h>
#include <pathsummary.h>
}
// C++ includes
#include <vtollandfsm.h>
// Private constants
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
#define MIN_LANDRATE 0.1f
#define MAX_LANDRATE 0.6f
#define LOW_ALT_DESCENT_REDUCTION_FACTOR 0.7f // TODO Need to make the transition smooth
#define LANDRATE_LOWLIMIT_FACTOR 0.5f
#define LANDRATE_HILIMIT_FACTOR 1.5f
#define TIMEOUT_INIT_ALTHOLD (3 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_WTG_FOR_DESCENTRATE (10 * TIMER_COUNT_PER_SECOND)
#define WTG_FOR_DESCENTRATE_COUNT_LIMIT 10
#define TIMEOUT_AT_DESCENTRATE 10
#define TIMEOUT_GROUNDEFFECT (1 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTDOWN (2 * TIMER_COUNT_PER_SECOND)
#define LANDING_PID_SCALAR_P 2.0f
#define LANDING_PID_SCALAR_I 10.0f
#define LANDING_SLOWDOWN_HEIGHT -5.0f
#define BOUNCE_VELOCITY_TRIGGER_LIMIT -0.3f
#define BOUNCE_ACCELERATION_TRIGGER_LIMIT -6.0f
#define BOUNCE_TRIGGER_COUNT 4
#define GROUNDEFFECT_SLOWDOWN_FACTOR 0.3f
#define GROUNDEFFECT_SLOWDOWN_COUNT 4
VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAND_STATE_SIZE] = {
[LAND_STATE_INACTIVE] = { .setup = &VtolLandFSM::setup_inactive, .run = 0 },
[LAND_STATE_INIT_ALTHOLD] = { .setup = &VtolLandFSM::setup_init_althold, .run = &VtolLandFSM::run_init_althold },
[LAND_STATE_WTG_FOR_DESCENTRATE] = { .setup = &VtolLandFSM::setup_wtg_for_descentrate, .run = &VtolLandFSM::run_wtg_for_descentrate },
[LAND_STATE_AT_DESCENTRATE] = { .setup = &VtolLandFSM::setup_at_descentrate, .run = &VtolLandFSM::run_at_descentrate },
[LAND_STATE_WTG_FOR_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_wtg_for_groundeffect, .run = &VtolLandFSM::run_wtg_for_groundeffect },
[LAND_STATE_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_groundeffect, .run = &VtolLandFSM::run_groundeffect },
[LAND_STATE_THRUSTDOWN] = { .setup = &VtolLandFSM::setup_thrustdown, .run = &VtolLandFSM::run_thrustdown },
[LAND_STATE_THRUSTOFF] = { .setup = &VtolLandFSM::setup_thrustoff, .run = &VtolLandFSM::run_thrustoff },
[LAND_STATE_DISARMED] = { .setup = &VtolLandFSM::setup_disarmed, .run = &VtolLandFSM::run_disarmed },
[LAND_STATE_ABORT] = { .setup = &VtolLandFSM::setup_abort, .run = &VtolLandFSM::run_abort }
};
// pointer to a singleton instance
VtolLandFSM *VtolLandFSM::p_inst = 0;
VtolLandFSM::VtolLandFSM()
: mLandData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
{}
// Private types
// Private functions
// Public API methods
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
PIOS_Assert(ptr_pathDesired);
PIOS_Assert(ptr_flightStatus);
if (mLandData == 0) {
mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T));
PIOS_Assert(mLandData);
}
memset(mLandData, sizeof(VtolLandFSMData_T), 0);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
initFSM();
return 0;
}
void VtolLandFSM::Inactive(void)
{
memset(mLandData, sizeof(VtolLandFSMData_T), 0);
initFSM();
}
// Initialise the FSM
void VtolLandFSM::initFSM(void)
{
if (vtolPathFollowerSettings != 0) {
setState(LAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE);
} else {
mLandData->currentState = LAND_STATE_INACTIVE;
}
}
void VtolLandFSM::Activate()
{
memset(mLandData, sizeof(VtolLandFSMData_T), 0);
mLandData->currentState = LAND_STATE_INACTIVE;
mLandData->flLowAltitude = false;
mLandData->flAltitudeHold = false;
mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
TakeOffLocationGet(&(mLandData->takeOffLocation));
mLandData->fsmLandStatus.AltitudeAtState[LAND_STATE_INACTIVE] = 0.0f;
assessAltitude();
if (pathDesired->Mode == PATHDESIRED_MODE_LAND) {
#ifndef DEBUG_GROUNDIMPACT
setState(LAND_STATE_INIT_ALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE);
#else
setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
#endif
} else {
// move to error state and callback to position hold
setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
}
void VtolLandFSM::Abort(void)
{
setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void)
{
switch (mLandData->currentState) {
case LAND_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
case LAND_STATE_ABORT:
return PFFSM_STATE_ABORT;
break;
case LAND_STATE_DISARMED:
return PFFSM_STATE_DISARMED;
break;
default:
return PFFSM_STATE_ACTIVE;
break;
}
}
void VtolLandFSM::Update()
{
runState();
if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
runAlways();
}
}
int32_t VtolLandFSM::runState(void)
{
uint8_t flTimeout = false;
mLandData->stateRunCount++;
if (mLandData->stateTimeoutCount > 0 && mLandData->stateRunCount > mLandData->stateTimeoutCount) {
flTimeout = true;
}
// If the current state has a static function, call it
if (sLandStateTable[mLandData->currentState].run) {
(this->*sLandStateTable[mLandData->currentState].run)(flTimeout);
}
return 0;
}
int32_t VtolLandFSM::runAlways(void)
{
void assessAltitude(void);
return 0;
}
// PathFollower implements the PID scheme and has a objective
// set by a PathDesired object. Based on the mode, pathfollower
// uses FSM's as helper functions that manage state and event detection.
// PathFollower calls into FSM methods to alter its commands.
void VtolLandFSM::BoundThrust(float &ulow, float &uhigh)
{
ulow = mLandData->boundThrustMin;
uhigh = mLandData->boundThrustMax;
if (mLandData->flConstrainThrust) {
uhigh = mLandData->thrustLimit;
}
}
void VtolLandFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
{
if (mLandData->flZeroStabiHorizontal && stabDesired) {
stabDesired->Pitch = 0.0f;
stabDesired->Roll = 0.0f;
stabDesired->Yaw = 0.0f;
}
}
void VtolLandFSM::CheckPidScaler(pid_scaler *local_scaler)
{
if (mLandData->flLowAltitude) {
local_scaler->p = LANDING_PID_SCALAR_P;
local_scaler->i = LANDING_PID_SCALAR_I;
}
}
// Set the new state and perform setup for subsequent state run calls
// This is called by state run functions on event detection that drive
// state transitions.
void VtolLandFSM::setState(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason)
{
mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason;
if (mLandData->currentState == newState) {
return;
}
mLandData->currentState = newState;
if (newState != LAND_STATE_INACTIVE) {
PositionStateData positionState;
PositionStateGet(&positionState);
float takeOffDown = 0.0f;
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mLandData->takeOffLocation.Down;
}
mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
assessAltitude();
}
// Restart state timer counter
mLandData->stateRunCount = 0;
// Reset state timeout to disabled/zero
mLandData->stateTimeoutCount = 0;
if (sLandStateTable[mLandData->currentState].setup) {
(this->*sLandStateTable[mLandData->currentState].setup)();
}
updateVtolLandFSMStatus();
}
// Timeout utility function for use by state init implementations
void VtolLandFSM::setStateTimeout(int32_t count)
{
mLandData->stateTimeoutCount = count;
}
void VtolLandFSM::updateVtolLandFSMStatus()
{
mLandData->fsmLandStatus.State = mLandData->currentState;
if (mLandData->flLowAltitude) {
mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_LOW;
} else {
mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_HIGH;
}
StatusVtolLandSet(&mLandData->fsmLandStatus);
}
float VtolLandFSM::BoundVelocityDown(float velocity_down)
{
velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE);
if (mLandData->flLowAltitude) {
velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR;
}
mLandData->fsmLandStatus.targetDescentRate = velocity_down;
if (mLandData->flAltitudeHold) {
return 0.0f;
} else {
return velocity_down;
}
}
void VtolLandFSM::assessAltitude(void)
{
float positionDown;
PositionStateDownGet(&positionDown);
float takeOffDown = 0.0f;
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mLandData->takeOffLocation.Down;
}
float positionDownRelativeToTakeoff = positionDown - takeOffDown;
if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) {
mLandData->flLowAltitude = false;
} else {
mLandData->flLowAltitude = true;
}
}
// FSM Setup and Run method implementation
// State: INACTIVE
void VtolLandFSM::setup_inactive(void)
{
// Re-initialise local variables
mLandData->flZeroStabiHorizontal = false;
mLandData->flConstrainThrust = false;
}
// State: INIT ALTHOLD
void VtolLandFSM::setup_init_althold(void)
{
setStateTimeout(TIMEOUT_INIT_ALTHOLD);
// get target descent velocity
mLandData->flZeroStabiHorizontal = false;
mLandData->fsmLandStatus.targetDescentRate = BoundVelocityDown(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]);
mLandData->flConstrainThrust = false;
mLandData->flAltitudeHold = true;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_init_althold(uint8_t flTimeout)
{
if (flTimeout) {
mLandData->flAltitudeHold = false;
setState(LAND_STATE_WTG_FOR_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// State: WAITING FOR DESCENT RATE
void VtolLandFSM::setup_wtg_for_descentrate(void)
{
setStateTimeout(TIMEOUT_WTG_FOR_DESCENTRATE);
// get target descent velocity
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->observation2Count = 0;
mLandData->flConstrainThrust = false;
mLandData->flAltitudeHold = false;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout)
{
// Look at current actual thrust...are we already shutdown??
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// We don't expect PID to get exactly the target descent rate, so have a lower
// water mark but need to see 5 observations to be confident that we have semi-stable
// descent achieved
// we need to see velocity down within a range of control before we proceed, without which we
// really don't have confidence to allow later states to run.
if (velocityState.Down > (LANDRATE_LOWLIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate) &&
velocityState.Down < (LANDRATE_HILIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate)) {
if (mLandData->observationCount++ > WTG_FOR_DESCENTRATE_COUNT_LIMIT) {
setState(LAND_STATE_AT_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
return;
}
}
if (flTimeout) {
setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// State: AT DESCENT RATE
void VtolLandFSM::setup_at_descentrate(void)
{
setStateTimeout(TIMEOUT_AT_DESCENTRATE);
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->sum1 = 0.0f;
mLandData->sum2 = 0.0f;
mLandData->flConstrainThrust = false;
mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_at_descentrate(uint8_t flTimeout)
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mLandData->sum1 += velocityState.Down;
mLandData->sum2 += stabDesired.Thrust;
mLandData->observationCount++;
if (flTimeout) {
mLandData->fsmLandStatus.averageDescentRate = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE);
mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
// We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground
// As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can
// detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value
mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust;
mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max);
setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
}
}
// State: WAITING FOR GROUND EFFECT
void VtolLandFSM::setup_wtg_for_groundeffect(void)
{
// No timeout
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->observation2Count = 0;
mLandData->sum1 = 0.0f;
mLandData->sum2 = 0.0f;
mLandData->flConstrainThrust = false;
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTimeout)
{
// detect material downrating in thrust for 1 second.
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
AccelStateData accelState;
AccelStateGet(&accelState);
// +ve 9.8 expected
float g_e;
HomeLocationg_eGet(&g_e);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// detect bounce
uint8_t flBounce = (velocityState.Down < BOUNCE_VELOCITY_TRIGGER_LIMIT);
if (flBounce) {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = velocityState.Down;
} else {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
}
// invert sign of accel to the standard convention of down is +ve and subtract the gravity to get
// a relative acceleration term.
float bounceAccel = -accelState.z - g_e;
uint8_t flBounceAccel = (bounceAccel < BOUNCE_ACCELERATION_TRIGGER_LIMIT);
if (flBounceAccel) {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = bounceAccel;
} else {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
}
if (flBounce || flBounceAccel) {
mLandData->observation2Count++;
if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) {
setState(LAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL));
return;
}
} else {
mLandData->observation2Count = 0;
}
// detect low descent rate
uint8_t flDescentRateLow = (velocityState.Down < (GROUNDEFFECT_SLOWDOWN_FACTOR * mLandData->fsmLandStatus.averageDescentRate));
if (flDescentRateLow) {
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
mLandData->observationCount++;
if (mLandData->observationCount > GROUNDEFFECT_SLOWDOWN_COUNT) {
#ifndef DEBUG_GROUNDIMPACT
setState(LAND_STATE_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_LOWDESCENTRATE);
#endif
return;
}
} else {
mLandData->observationCount = 0;
}
updateVtolLandFSMStatus();
}
// STATE: GROUNDEFFET
void VtolLandFSM::setup_groundeffect(void)
{
setStateTimeout(TIMEOUT_GROUNDEFFECT);
mLandData->flZeroStabiHorizontal = true;
PositionStateData positionState;
PositionStateGet(&positionState);
mLandData->expectedLandPositionNorth = positionState.North;
mLandData->expectedLandPositionEast = positionState.East;
mLandData->flConstrainThrust = false;
// now that we have ground effect limit max thrust to neutral
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
}
void VtolLandFSM::run_groundeffect(__attribute__((unused)) uint8_t flTimeout)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f) {
setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
return;
}
// Stay in this state until we get a low altitude flag.
if (mLandData->flLowAltitude == false) {
// worst case scenario is that we land and the pid brings thrust down to zero.
return;
}
// detect broad sideways drift. If for some reason we have a hard landing that the bounce detection misses, this will kick in
PositionStateData positionState;
PositionStateGet(&positionState);
float north_error = mLandData->expectedLandPositionNorth - positionState.North;
float east_error = mLandData->expectedLandPositionEast - positionState.East;
float positionError = sqrtf(north_error * north_error + east_error * east_error);
if (positionError > 0.3f) {
setState(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR);
return;
}
if (flTimeout) {
setState(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTDOWN
void VtolLandFSM::setup_thrustdown(void)
{
setStateTimeout(TIMEOUT_THRUSTDOWN);
mLandData->flZeroStabiHorizontal = true;
mLandData->flConstrainThrust = true;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mLandData->thrustLimit = stabDesired.Thrust;
mLandData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
}
void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
{
// reduce thrust setpoint step by step
mLandData->thrustLimit -= mLandData->sum1;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f || mLandData->thrustLimit < 0.0f) {
setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
}
if (flTimeout) {
setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTOFF
void VtolLandFSM::setup_thrustoff(void)
{
mLandData->thrustLimit = -1.0f;
mLandData->flConstrainThrust = true;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = 0.0f;
}
void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
{
setState(LAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
// STATE: DISARMED
void VtolLandFSM::setup_disarmed(void)
{
// nothing to do
mLandData->flConstrainThrust = false;
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = 0.0f;
}
void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
{
#ifdef DEBUG_GROUNDIMPACT
if (mLandData->observationCount++ > 100) {
setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
#endif
}
void VtolLandFSM::fallback_to_hold(void)
{
PositionStateData positionState;
PositionStateGet(&positionState);
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(pathDesired);
}
// abort repeatedly overwrites pathfollower's objective on a landing abort and
// continues to do so until a flight mode change.
void VtolLandFSM::setup_abort(void)
{
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
mLandData->flConstrainThrust = false;
mLandData->flZeroStabiHorizontal = false;
fallback_to_hold();
}
void VtolLandFSM::run_abort(__attribute__((unused)) uint8_t flTimeout)
{}

227
flight/modules/PathFollower/vtolvelocitycontroller.cpp Normal file
View File

@ -0,0 +1,227 @@
/*
******************************************************************************
*
* @file VtolVelocityController.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Velocity roam controller for vtols
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <flightstatus.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtolvelocitycontroller.h"
#include "pidcontrolne.h"
// Private constants
// pointer to a singleton instance
VtolVelocityController *VtolVelocityController::p_inst = 0;
VtolVelocityController::VtolVelocityController()
: vtolPathFollowerSettings(0), mActive(false)
{}
// Called when mode first engaged
void VtolVelocityController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
controlNE.Activate();
}
}
uint8_t VtolVelocityController::IsActive(void)
{
return mActive;
}
uint8_t VtolVelocityController::Mode(void)
{
return PATHDESIRED_MODE_VELOCITY;
}
void VtolVelocityController::ObjectiveUpdated(void)
{
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
}
void VtolVelocityController::Deactivate(void)
{
if (mActive) {
mActive = false;
controlNE.Deactivate();
}
}
void VtolVelocityController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.ILimit,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
}
int32_t VtolVelocityController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
return 0;
}
void VtolVelocityController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
velocityDesired.Down = 0.0f;
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
// update pathstatus
pathStatus->error = 0.0f;
pathStatus->fractional_progress = 0.0f;
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus->correction_direction_down = 0.0f;
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)) bool yaw_attitude, __attribute__((unused)) float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
// default thrust mode to altvario
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO;
StabilizationDesiredSet(&stabDesired);
return result;
}
void VtolVelocityController::UpdateAutoPilot()
{
UpdateVelocityDesired();
bool yaw_attitude = false;
float yaw = 0.0f;
int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
fallback_to_hold();
}
PathStatusSet(pathStatus);
}
void VtolVelocityController::fallback_to_hold(void)
{
PositionStateData positionState;
PositionStateGet(&positionState);
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(pathDesired);
}

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

@ -43,8 +43,11 @@
#include "waypoint.h"
#include "waypointactive.h"
#include "flightmodesettings.h"
#include <systemsettings.h>
#include "paths.h"
#include "plans.h"
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
// Private constants
#define STACK_SIZE_BYTES 1024
@ -73,6 +76,7 @@ static uint8_t conditionAboveSpeed();
static uint8_t conditionPointingTowardsNext();
static uint8_t conditionPythonScript();
static uint8_t conditionImmediate();
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
// Private variables
@ -82,7 +86,10 @@ static WaypointActiveData waypointActive;
static WaypointData waypoint;
static PathActionData pathAction;
static bool pathplanner_active = false;
static FrameType_t frameType;
static bool mode3D;
extern FrameType_t GetCurrentFrameType();
/**
* Module initialization
@ -95,6 +102,9 @@ int32_t PathPlannerStart()
WaypointActiveConnectCallback(commandUpdated);
PathActionConnectCallback(commandUpdated);
PathStatusConnectCallback(statusUpdated);
SettingsUpdatedCb(NULL);
SystemSettingsConnectCallback(&SettingsUpdatedCb);
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
// Start main task callback
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
@ -125,6 +135,34 @@ int32_t PathPlannerInitialize()
MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
frameType = GetCurrentFrameType();
if (frameType == FRAME_TYPE_CUSTOM) {
switch (TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
mode3D = true;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
mode3D = true;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
mode3D = false;
break;
}
}
}
/**
* Module task
*/
@ -160,22 +198,17 @@ static void pathPlannerTask()
static uint8_t failsafeRTHset = 0;
if (!validPathPlan) {
// At this point the craft is in PathPlanner mode, so pilot has no manual control capability.
// Failsafe: behave as if in return to base mode
// what to do in this case is debatable, it might be better to just
// make a forced disarming but rth has a higher chance of survival when
// in flight.
pathplanner_active = false;
if (!failsafeRTHset) {
failsafeRTHset = 1;
// copy pasta: same calculation as in manualcontrol, set return to home coordinates
plan_setup_positionHold();
}
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
failsafeRTHset = 0;
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
@ -525,7 +558,7 @@ static uint8_t conditionLegRemaining()
struct path_status progress;
path_progress(&pathDesired,
cur, &progress);
cur, &progress, mode3D);
if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) {
return true;
}
@ -549,7 +582,7 @@ static uint8_t conditionBelowError()
struct path_status progress;
path_progress(&pathDesired,
cur, &progress);
cur, &progress, mode3D);
if (progress.error <= pathAction.ConditionParameters[0]) {
return true;
}

2
flight/pios/inc/pios_posix.h
View File

@ -28,7 +28,9 @@
#include <stdint.h>
#ifndef __cplusplus
typedef enum { FALSE = 0, TRUE = !FALSE } bool;
#endif
#ifndef false
#define false FALSE

2
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -19,6 +19,7 @@
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
@ -61,6 +62,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += groundpathfollowersettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand

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

@ -19,6 +19,9 @@
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
@ -61,6 +64,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += groundpathfollowersettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand

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

@ -19,6 +19,9 @@
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
@ -61,6 +64,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += groundpathfollowersettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand

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

@ -20,6 +20,9 @@
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
# REVO C++ support
USE_CXX = YES
override ARM_SDK_PREFIX :=
override THUMB :=
@ -83,10 +86,11 @@ endif
## MODULES
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
CPPSRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.cpp}}
SRC += ${OUTDIR}/InitMods.c
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += $(OPSYSTEM)/simposix.c
CPPSRC += $(OPSYSTEM)/simposix.cpp
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(FLIGHTLIB)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
@ -124,6 +128,7 @@ SRC += $(PIOSCORECOMMON)/pios_mem.c
## PIOS Hardware
include $(PIOS)/posix/library.mk
include ./UAVObjects.inc
SRC += $(UAVOBJSRC)
@ -315,7 +320,7 @@ endif
# Link: create ELF output file from object files.
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
$(eval $(call LINK_CXX_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
# Assemble: create object files from assembler source files.
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
@ -330,7 +335,7 @@ $(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CXX_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))

4
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -24,6 +24,9 @@
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
@ -62,6 +65,7 @@ UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += gpssettings
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += groundpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += homelocation

2
flight/targets/boards/simposix/firmware/simposix.c → flight/targets/boards/simposix/firmware/simposix.cpp
View File

@ -31,6 +31,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include "inc/openpilot.h"
#include <systemmod.h>
#include <uavobjectsinit.h>
@ -74,6 +75,7 @@ static void initTask(void *parameters);
/* Prototype of generated InitModules() function */
extern void InitModules(void);
}
/**
* OpenPilot Main function:

2
ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp
View File

@ -138,7 +138,7 @@ void MagicWaypointGadgetWidget::positionSelected(double north, double east)
pathDesired.End[PathDesired::END_NORTH] = north * scale;
pathDesired.End[PathDesired::END_EAST] = east * scale;
pathDesired.Mode = PathDesired::MODE_FLYENDPOINT;
pathDesired.Mode = PathDesired::MODE_GOTOENDPOINT;
getPathDesired()->setData(pathDesired);
}

15
ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp
View File

@ -86,18 +86,17 @@ void modelMapProxy::selectedWPChanged(QList<WayPointItem *> list)
modelMapProxy::overlayType modelMapProxy::overlayTranslate(int type)
{
switch (type) {
case MapDataDelegate::MODE_FLYENDPOINT:
case MapDataDelegate::MODE_FLYVECTOR:
case MapDataDelegate::MODE_DRIVEENDPOINT:
case MapDataDelegate::MODE_DRIVEVECTOR:
case MapDataDelegate::MODE_GOTOENDPOINT:
case MapDataDelegate::MODE_FOLLOWVECTOR:
case MapDataDelegate::MODE_VELOCITY:
case MapDataDelegate::MODE_LAND:
case MapDataDelegate::MODE_BRAKE:
return OVERLAY_LINE;
case MapDataDelegate::MODE_FLYCIRCLERIGHT:
case MapDataDelegate::MODE_DRIVECIRCLERIGHT:
case MapDataDelegate::MODE_CIRCLERIGHT:
return OVERLAY_CIRCLE_RIGHT;
case MapDataDelegate::MODE_FLYCIRCLELEFT:
case MapDataDelegate::MODE_DRIVECIRCLELEFT:
case MapDataDelegate::MODE_CIRCLELEFT:
default:
return OVERLAY_CIRCLE_LEFT;
}

24
ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp
View File

@ -104,15 +104,13 @@ void opmap_edit_waypoint_dialog::setupModeWidgets()
MapDataDelegate::ModeOptions mode = (MapDataDelegate::ModeOptions)ui->cbMode->itemData(ui->cbMode->currentIndex()).toInt();
switch (mode) {
case MapDataDelegate::MODE_FLYENDPOINT:
case MapDataDelegate::MODE_FLYVECTOR:
case MapDataDelegate::MODE_FLYCIRCLERIGHT:
case MapDataDelegate::MODE_FLYCIRCLELEFT:
case MapDataDelegate::MODE_DRIVEENDPOINT:
case MapDataDelegate::MODE_DRIVEVECTOR:
case MapDataDelegate::MODE_DRIVECIRCLELEFT:
case MapDataDelegate::MODE_DRIVECIRCLERIGHT:
case MapDataDelegate::MODE_GOTOENDPOINT:
case MapDataDelegate::MODE_FOLLOWVECTOR:
case MapDataDelegate::MODE_CIRCLERIGHT:
case MapDataDelegate::MODE_CIRCLELEFT:
case MapDataDelegate::MODE_DISARMALARM:
case MapDataDelegate::MODE_LAND:
case MapDataDelegate::MODE_BRAKE:
ui->modeParam1->setVisible(false);
ui->modeParam2->setVisible(false);
ui->modeParam3->setVisible(false);
@ -122,6 +120,16 @@ void opmap_edit_waypoint_dialog::setupModeWidgets()
ui->dsb_modeParam3->setVisible(false);
ui->dsb_modeParam4->setVisible(false);
break;
case MapDataDelegate::MODE_VELOCITY:
ui->modeParam1->setVisible(true);
ui->modeParam2->setVisible(true);
ui->modeParam3->setVisible(true);
ui->modeParam4->setVisible(false);
ui->dsb_modeParam1->setVisible(true);
ui->dsb_modeParam2->setVisible(true);
ui->dsb_modeParam3->setVisible(true);
ui->dsb_modeParam4->setVisible(false);
break;
case MapDataDelegate::MODE_FIXEDATTITUDE:
ui->modeParam1->setText("pitch");
ui->modeParam2->setText("roll");

18
ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp
View File

@ -106,19 +106,17 @@ void MapDataDelegate::loadComboBox(QComboBox *combo, flightDataModel::pathPlanDa
{
switch (type) {
case flightDataModel::MODE:
combo->addItem("Fly Direct", MODE_FLYENDPOINT);
combo->addItem("Fly Vector", MODE_FLYVECTOR);
combo->addItem("Fly Circle Right", MODE_FLYCIRCLERIGHT);
combo->addItem("Fly Circle Left", MODE_FLYCIRCLELEFT);
combo->addItem("Drive Direct", MODE_DRIVEENDPOINT);
combo->addItem("Drive Vector", MODE_DRIVEVECTOR);
combo->addItem("Drive Circle Right", MODE_DRIVECIRCLELEFT);
combo->addItem("Drive Circle Left", MODE_DRIVECIRCLERIGHT);
combo->addItem("Goto Endpoint", MODE_GOTOENDPOINT);
combo->addItem("Follow Vector", MODE_FOLLOWVECTOR);
combo->addItem("Circle Right", MODE_CIRCLERIGHT);
combo->addItem("Circle Left", MODE_CIRCLELEFT);
combo->addItem("Fixed Attitude", MODE_FIXEDATTITUDE);
combo->addItem("Set Accessory", MODE_SETACCESSORY);
combo->addItem("Disarm Alarm", MODE_DISARMALARM);
combo->addItem("Land", MODE_LAND);
combo->addItem("Brake", MODE_BRAKE);
combo->addItem("Velocity", MODE_VELOCITY);
break;
case flightDataModel::CONDITION:
combo->addItem("None", ENDCONDITION_NONE);

7
ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h
View File

@ -36,9 +36,10 @@ class MapDataDelegate : public QItemDelegate {
Q_OBJECT
public:
typedef enum { MODE_FLYENDPOINT = 0, MODE_FLYVECTOR = 1, MODE_FLYCIRCLERIGHT = 2, MODE_FLYCIRCLELEFT = 3,
MODE_DRIVEENDPOINT = 4, MODE_DRIVEVECTOR = 5, MODE_DRIVECIRCLELEFT = 6, MODE_DRIVECIRCLERIGHT = 7,
MODE_FIXEDATTITUDE = 8, MODE_SETACCESSORY = 9, MODE_DISARMALARM = 10 } ModeOptions;
typedef enum { MODE_GOTOENDPOINT = 0, MODE_FOLLOWVECTOR = 1, MODE_CIRCLERIGHT = 2, MODE_CIRCLELEFT = 3,
MODE_FIXEDATTITUDE = 8, MODE_SETACCESSORY = 9, MODE_DISARMALARM = 10, MODE_LAND = 11,
MODE_BRAKE = 12, MODE_VELOCITY = 13} ModeOptions;
typedef enum { ENDCONDITION_NONE = 0, ENDCONDITION_TIMEOUT = 1, ENDCONDITION_DISTANCETOTARGET = 2,
ENDCONDITION_LEGREMAINING = 3, ENDCONDITION_BELOWERROR = 4, ENDCONDITION_ABOVEALTITUDE = 5,
ENDCONDITION_ABOVESPEED = 6, ENDCONDITION_POINTINGTOWARDSNEXT = 7, ENDCONDITION_PYTHONSCRIPT = 8,

8
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -27,6 +27,9 @@ OTHER_FILES += UAVObjects.pluginspec
# Add in all of the synthetic/generated uavobject files
HEADERS += \
$$UAVOBJECT_SYNTHETICS/statusgrounddrive.h \
$$UAVOBJECT_SYNTHETICS/pidstatus.h \
$$UAVOBJECT_SYNTHETICS/statusvtolland.h \
$$UAVOBJECT_SYNTHETICS/vtolselftuningstats.h \
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.h \
$$UAVOBJECT_SYNTHETICS/accessorydesired.h \
@ -96,6 +99,7 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.h \
$$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.h \
$$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.h \
$$UAVOBJECT_SYNTHETICS/groundpathfollowersettings.h \
$$UAVOBJECT_SYNTHETICS/ratedesired.h \
$$UAVOBJECT_SYNTHETICS/firmwareiapobj.h \
$$UAVOBJECT_SYNTHETICS/i2cstats.h \
@ -133,6 +137,9 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/perfcounter.h
SOURCES += \
$$UAVOBJECT_SYNTHETICS/statusgrounddrive.cpp \
$$UAVOBJECT_SYNTHETICS/pidstatus.cpp \
$$UAVOBJECT_SYNTHETICS/statusvtolland.cpp \
$$UAVOBJECT_SYNTHETICS/vtolselftuningstats.cpp \
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
$$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
@ -202,6 +209,7 @@ SOURCES += \
$$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.cpp \
$$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.cpp \
$$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.cpp \
$$UAVOBJECT_SYNTHETICS/groundpathfollowersettings.cpp \
$$UAVOBJECT_SYNTHETICS/ratedesired.cpp \
$$UAVOBJECT_SYNTHETICS/firmwareiapobj.cpp \
$$UAVOBJECT_SYNTHETICS/i2cstats.cpp \

1
make/common-defs.mk
View File

@ -164,7 +164,6 @@ ifeq ($(USE_CXX), YES)
CPPFLAGS += -DPIOS_ENABLE_CXX
endif
# Compiler flags to generate dependency files
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d

2
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -2,7 +2,7 @@
<object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref AltitudeHold module</description>
<field name="AltitudePI" units="(m/s)/m" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.8,0,0" />
<field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.2,0.0002,2.0" />
<field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.3,0.3,2.0" />
<field name="CutThrustWhenZero" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" />
<field name="ThrustExp" units="" type="uint8" elements="1" defaultvalue="128" />
<field name="ThrustRate" units="m/s" type="float" elements="1" defaultvalue="5" />

15
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -83,34 +83,35 @@
limits="\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0903NE:POI:PathPlanner:AutoCruise:Land;\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0903NE:POI:PathPlanner:AutoCruise:Land;\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0903NE:POI:PathPlanner:AutoCruise:Land;\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0903NE:POI:PathPlanner:AutoCruise:Land;\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0903NE:POI:PathPlanner:AutoCruise:Land;\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0903NE:POI:PathPlanner:AutoCruise:Land;"/>
%0903NE:POI:PathPlanner:AutoCruise;"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="DisableSanityChecks" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="ReturnToBaseAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
<field name="LandingVelocity" units="m" type="float" elements="1" defaultvalue="0.4"/>
<field name="ReturnToBaseNextCommand" units="" type="enum" elements="1" options="Hold,Land" defaultvalue="Hold"/>
<field name="LandingVelocity" units="m" type="float" elements="1" defaultvalue="0.6"/>
<field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="30,15" description="stick sensitivity for position roam modes"/>
<field name="VarioControlLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.98" description="stick low pass filter for position roam modes"/>
<access gcs="readwrite" flight="readwrite"/>

30
shared/uavobjectdefinition/groundpathfollowersettings.xml Normal file
View File

@ -0,0 +1,30 @@
<xml>
<object name="GroundPathFollowerSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref GroundPathFollowerModule</description>
<!-- these coefficients control desired movement in airspace -->
<field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="2"/>
<!-- Maximum speed the autopilot will try to achieve, usually for long distances -->
<field name="HorizontalVelMin" units="m/s" type="float" elements="1" defaultvalue="0"/>
<!-- V_y, Minimum speed the autopilot will try to fly, for example when loitering -->
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="3.0"/>
<field name="VelocityFeedForward" units="s" type="float" elements="1" defaultvalue="0.1"/>
<!-- how many seconds to plan the flight vector ahead when initiating necessary heading changes - increase for planes with sluggish response -->
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.2"/>
<!-- proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations -->
<!-- these coefficients control actual control outputs -->
<field name="SpeedPI" units="deg / (m/s)" type="float" elements="4" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.1, .1, 0.001, 0.8"/>
<!-- coefficients for desired pitch
in relation to speed error IASerror -->
<field name="ThrustLimit" units="" type="float" elements="2" elementnames="Min,SlowForward, Max" defaultvalue="-0.3,0.15,0.3" />
<!-- minimum and maximum allowed thrust and setpoint for cruise speed -->
<field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

2
shared/uavobjectdefinition/nedaccel.xml
View File

@ -6,7 +6,7 @@
<field name="Down" units="m/s^2" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="10001"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

7
shared/uavobjectdefinition/pathaction.xml
View File

@ -2,9 +2,10 @@
<object name="PathAction" singleinstance="false" settings="false" category="Navigation">
<description>A waypoint command the pathplanner is to use at a certain waypoint</description>
<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
FixedAttitude, SetAccessory, DisarmAlarm" default="Endpoint" />
<!-- ensure the following Mode options are exactly the same as in pathdesired mode -->
<field name="Mode" units="" type="enum" elements="1" options="GoToEndpoint,FollowVector,CircleRight,CircleLeft,
FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity" default="GoToEndpoint" />
<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
DistanceToTarget,LegRemaining,BelowError,AboveAltitude,AboveSpeed,PointingTowardsNext,

12
shared/uavobjectdefinition/pathdesired.xml
View File

@ -3,16 +3,14 @@
<description>The endpoint or path the craft is trying to achieve. Can come from @ref ManualControl or @ref PathPlanner </description>
<field name="Start" units="m" type="float" elementnames="North,East,Down" default="0"/>
<field name="End" units="m" type="float" elementnames="North,East,Down" default="0"/>
<field name="End" units="m" type="float" elementnames="North,East,Down" default="0"/>
<field name="StartingVelocity" units="m/s" type="float" elements="1" default="0"/>
<field name="EndingVelocity" units="m/s" type="float" elements="1" default="0"/>
<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
FixedAttitude,
SetAccessory,
Land,
DisarmAlarm,Brake" default="0"/>
<!-- ensure the following Mode options are exactly the same as in pathaction mode -->
<field name="Mode" units="" type="enum" elements="1" options="GoToEndpoint,FollowVector,CircleRight,CircleLeft,
FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity" default="GoToEndpoint" />
<!-- Endpoint mode - move directly towards endpoint regardless of position -->
<!-- Straight Mode - move across linear path through Start towards the waypoint end, adjusting velocity - continue straight -->
<!-- Circle Mode - move a circular pattern around End with radius End-Start (straight line in the vertical)-->

2
shared/uavobjectdefinition/pathsummary.xml
View File

@ -11,6 +11,8 @@
<field name="decelrate" units="m/s2" type="float" elements="1"/>
<field name="brakeRateActualDesiredRatio" units="" type="float" elements="1"/>
<field name="velocityIntoHold" units="m/s" type="float" elements="1"/>
<field name="Mode" units="" type="enum" elements="1" options="GoToEndpoint,FollowVector,CircleRight,CircleLeft,
FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity" default="GoToEndpoint" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

18
shared/uavobjectdefinition/pidstatus.xml Normal file
View File

@ -0,0 +1,18 @@
<xml>
<object name="PIDStatus" singleinstance="true" settings="false" category="Navigation">
<description>Status of a PID loop for debugging</description>
<field name="setpoint" units="m" type="float" elements="1"/>
<field name="actual" units="m" type="float" elements="1"/>
<field name="error" units="m" type="float" elements="1"/>
<field name="ulow" units="m" type="float" elements="1"/>
<field name="uhigh" units="m" type="float" elements="1"/>
<field name="command" units="m" type="float" elements="1"/>
<field name="P" units="m" type="float" elements="1"/>
<field name="I" units="m" type="float" elements="1"/>
<field name="D" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

14
shared/uavobjectdefinition/statusgrounddrive.xml Normal file
View File

@ -0,0 +1,14 @@
<xml>
<object name="StatusGroundDrive" singleinstance="true" settings="false" category="Navigation">
<description>Status of a Ground drive sequence</description>
<field name="ControlState" units="" type="enum" elements="1" options="Inactive,OnTrack,TurnAroundRight,TurnAroundLeft,Brake" default="0"/>
<field name="NECommand" units="" type="float" elementnames="North,East" defaultvalue="0.0, 0.0"/>
<field name="State" units="" type="float" elementnames="Yaw,Velocity,Thrust" defaultvalue="0.0"/>
<field name="BodyCommand" units="" type="float" elementnames="Forward,Right" defaultvalue="0.0, 0.0"/>
<field name="ControlCommand" units="" type="float" elementnames="Speed,Course" defaultvalue="0.0, 0.0"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="onchange" period="100"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

19
shared/uavobjectdefinition/statusvtolland.xml Normal file
View File

@ -0,0 +1,19 @@
<xml>
<object name="StatusVtolLand" singleinstance="true" settings="false" category="Navigation">
<description>Status of a Vtol landing sequence</description>
<field name="State" units="" type="enum" elements="1" options="Inactive,InitAltHold,WtgForDescentRate,AtDescentRate,WtgForGroundEffect,
GroundEffect,ThrustDown,ThrustOff,Disarmed, Abort" default="0"/>
<field name="AltitudeAtState" units="m" type="float" elements="10" default="0"/>
<field name="StateExitReason" units="" type="enum" elements="10" options="None,DescentRateOk,OnGround,BounceVelocity,BounceAccel,LowDescentRate,ZeroThrust,PositionError,Timeout" default="0"/>
<field name="targetDescentRate" units="m" type="float" elements="1"/>
<field name="averageDescentRate" units="m" type="float" elements="1"/>
<field name="averageDescentThrust" units="m" type="float" elements="1"/>
<field name="calculatedNeutralThrust" units="m" type="float" elements="1"/>
<field name="AltitudeState" units="" type="enum" elements="1" options="High,Low" default="0"/>
<field name="WtgForGroundEffect" units="" type="float" elementnames="BounceVelocity,BounceAccel" defaultvalue="0.0, 0.0"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="onchange" period="100"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

3
shared/uavobjectdefinition/vtolpathfollowersettings.xml
View File

@ -8,7 +8,7 @@
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.25"/>
<field name="VerticalPosP" units="" type="float" elements="1" defaultvalue="0.4"/>
<field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="8.0, 0.5, 0.0, 15"/>
<field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1, 0.01, 0.0, 1.0"/>
<field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.3, 0.3, 0.0, 1.0"/>
<field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>
<field name="VelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="2"/>
<field name="ThrustControl" units="" type="enum" elements="1" options="manual,auto" defaultvalue="manual"/>
@ -23,6 +23,7 @@
<field name="BrakeMaxPitch" units="deg" type="float" elements="1" defaultvalue="25"/>
<field name="BrakeHorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="12.0, 0.0, 0.03, 15"/>
<field name="BrakeVelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="0"/>
<field name="LandVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.35, 3.0, 0.05, 0.9"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>