From 112a8d9a8e7b94506f061084436c6f807fbe3a0e Mon Sep 17 00:00:00 2001 From: Young Nielson Date: Mon, 27 Jun 2022 13:39:52 -0500 Subject: [PATCH 1/4] Removed tasks and merged updated conf table and ECL controllers --- apps/fac/fsw/for_build/CMakeLists.txt | 30 +- .../{fac_cds_utils.hpp => ECL_Controller.cpp} | 14 +- apps/fac/fsw/src/ECL_Controller.hpp | 45 + ...nfig_utils.hpp => ECL_PitchController.cpp} | 17 +- apps/fac/fsw/src/ECL_PitchController.hpp | 46 + apps/fac/fsw/src/ECL_RollController.cpp | 45 + apps/fac/fsw/src/ECL_RollController.hpp | 46 + apps/fac/fsw/src/ECL_WheelController.cpp | 45 + apps/fac/fsw/src/ECL_WheelController.hpp | 46 + apps/fac/fsw/src/ECL_YawController.cpp | 45 + apps/fac/fsw/src/ECL_YawController.hpp | 46 + apps/fac/fsw/src/fac_app.cpp | 756 +++++++-- apps/fac/fsw/src/fac_app.hpp | 9 +- apps/fac/fsw/src/fac_cds_task.cpp | 478 ------ apps/fac/fsw/src/fac_cds_utils.cpp | 109 -- .../{fac_cmds_task.cpp => fac_cmds_utils.cpp} | 196 +-- .../{fac_cmds_task.hpp => fac_cmds_utils.hpp} | 16 +- apps/fac/fsw/src/fac_config_utils.cpp | 202 --- apps/fac/fsw/src/fac_data_utils.cpp | 539 +++++++ .../{fac_cds_task.hpp => fac_data_utils.hpp} | 80 +- apps/fac/fsw/src/fac_events.h | 17 - apps/fac/fsw/src/fac_fac.hpp | 26 +- apps/fac/fsw/src/fac_msg.h | 21 - apps/fac/fsw/src/fac_private_ids.h | 35 - apps/fac/fsw/src/fac_private_types.h | 8 +- apps/fac/fsw/src/fac_symbols.cpp | 33 + apps/fac/fsw/src/fac_tbldefs.h | 675 +++++++- apps/fac/fsw/src/fac_version.h | 5 - apps/fac/fsw/tables/fac_config.c | 670 +++++++- apps/fac/fsw/unit_test/fac_test_utils.c | 2 +- config/shared/apps/fac/tables/fac_config.c | 670 +++++++- config/shared/inc/px4_msgs.h | 1382 +++++++++++++++++ 32 files changed, 4854 insertions(+), 1500 deletions(-) rename apps/fac/fsw/src/{fac_cds_utils.hpp => ECL_Controller.cpp} (90%) create mode 100644 apps/fac/fsw/src/ECL_Controller.hpp rename apps/fac/fsw/src/{fac_config_utils.hpp => ECL_PitchController.cpp} (86%) create mode 100644 apps/fac/fsw/src/ECL_PitchController.hpp create mode 100644 apps/fac/fsw/src/ECL_RollController.cpp create mode 100644 apps/fac/fsw/src/ECL_RollController.hpp create mode 100644 apps/fac/fsw/src/ECL_WheelController.cpp create mode 100644 apps/fac/fsw/src/ECL_WheelController.hpp create mode 100644 apps/fac/fsw/src/ECL_YawController.cpp create mode 100644 apps/fac/fsw/src/ECL_YawController.hpp delete mode 100644 apps/fac/fsw/src/fac_cds_task.cpp delete mode 100644 apps/fac/fsw/src/fac_cds_utils.cpp rename apps/fac/fsw/src/{fac_cmds_task.cpp => fac_cmds_utils.cpp} (54%) rename apps/fac/fsw/src/{fac_cmds_task.hpp => fac_cmds_utils.hpp} (87%) delete mode 100644 apps/fac/fsw/src/fac_config_utils.cpp create mode 100644 apps/fac/fsw/src/fac_data_utils.cpp rename apps/fac/fsw/src/{fac_cds_task.hpp => fac_data_utils.hpp} (67%) create mode 100644 config/shared/inc/px4_msgs.h diff --git a/apps/fac/fsw/for_build/CMakeLists.txt b/apps/fac/fsw/for_build/CMakeLists.txt index 03ec89ada..78c1041bf 100644 --- a/apps/fac/fsw/for_build/CMakeLists.txt +++ b/apps/fac/fsw/for_build/CMakeLists.txt @@ -37,15 +37,21 @@ buildliner_add_app_def(fac ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_app.cpp ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_app.hpp ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_fac.hpp - ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_cds_task.cpp - ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_cds_task.hpp - ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_cds_utils.cpp - ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_cds_utils.hpp - ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_cmds_task.cpp - ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_cmds_task.hpp - ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_config_utils.cpp - ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_config_utils.hpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_cmds_utils.cpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_cmds_utils.hpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_data_utils.cpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_data_utils.hpp ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_symbols.cpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_Controller.cpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_Controller.hpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_PitchController.cpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_PitchController.hpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_RollController.cpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_RollController.hpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_WheelController.cpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_WheelController.hpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_YawController.cpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_YawController.cpp ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_events.h ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_msg.h ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_private_ids.h @@ -88,10 +94,10 @@ buildliner_add_app_unit_test(FAC-UT ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_app.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_app.hpp ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_fac.hpp - ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_cds_utils.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_cds_utils.hpp - ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_config_utils.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_config_utils.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_cmds_utils.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_cmds_utils.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_data_utils.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../src/fac_data_utils.hpp INCLUDES ${CMAKE_CURRENT_SOURCE_DIR}/../src/ diff --git a/apps/fac/fsw/src/fac_cds_utils.hpp b/apps/fac/fsw/src/ECL_Controller.cpp similarity index 90% rename from apps/fac/fsw/src/fac_cds_utils.hpp rename to apps/fac/fsw/src/ECL_Controller.cpp index 6026dafaa..d5af3978e 100644 --- a/apps/fac/fsw/src/fac_cds_utils.hpp +++ b/apps/fac/fsw/src/ECL_Controller.cpp @@ -31,15 +31,15 @@ * *****************************************************************************/ -#ifndef FAC_CDS_UTILS_HPP -#define FAC_CDS_UTILS_HPP +#include "ECL_Controller.hpp" -#include "fac_tbldefs.h" -int32 FAC_InitCdsTbl(); +ECL_Controller::ECL_Controller() +{ -void FAC_UpdateCdsTbl(void); +} -void FAC_SaveCdsTbl(void); +ECL_Controller::~ECL_Controller() +{ -#endif /* FAC_CDS_UTILS_HPP */ +} diff --git a/apps/fac/fsw/src/ECL_Controller.hpp b/apps/fac/fsw/src/ECL_Controller.hpp new file mode 100644 index 000000000..40a9b5d72 --- /dev/null +++ b/apps/fac/fsw/src/ECL_Controller.hpp @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + +#ifndef ECL_CONTROLLER_HPP +#define ECL_CONTROLLER_HPP + + +class ECL_Controller +{ +public: + ECL_Controller(); + ~ECL_Controller(); +}; + +#endif diff --git a/apps/fac/fsw/src/fac_config_utils.hpp b/apps/fac/fsw/src/ECL_PitchController.cpp similarity index 86% rename from apps/fac/fsw/src/fac_config_utils.hpp rename to apps/fac/fsw/src/ECL_PitchController.cpp index e4e12d557..428ef9520 100644 --- a/apps/fac/fsw/src/fac_config_utils.hpp +++ b/apps/fac/fsw/src/ECL_PitchController.cpp @@ -31,18 +31,15 @@ * *****************************************************************************/ -#ifndef FAC_CONFIG_UTILS_HPP -#define FAC_CONFIG_UTILS_HPP +#include "ECL_PitchController.hpp" -#include "cfe.h" -#include "fac_app.hpp" -int32 FAC_InitConfigTbl(); +ECL_PitchController::ECL_PitchController() +{ -int32 FAC_ValidateConfigTbl(void*); +} -int32 FAC_AcquireConfigPointers(); +ECL_PitchController::~ECL_PitchController() +{ -void FAC_ProcessNewConfigTbl(); - -#endif /* FAC_CONFIG_UTILS_HPP */ +} diff --git a/apps/fac/fsw/src/ECL_PitchController.hpp b/apps/fac/fsw/src/ECL_PitchController.hpp new file mode 100644 index 000000000..911aa4a77 --- /dev/null +++ b/apps/fac/fsw/src/ECL_PitchController.hpp @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + +#ifndef ECL_PITCHCONTROLLER_HPP +#define ECL_PITCHCONTROLLER_HPP + +#include "ECL_Controller.hpp" + +class ECL_PitchController : public ECL_Controller +{ +public: + ECL_PitchController(); + ~ECL_PitchController(); +}; + +#endif diff --git a/apps/fac/fsw/src/ECL_RollController.cpp b/apps/fac/fsw/src/ECL_RollController.cpp new file mode 100644 index 000000000..c9fdf8aa5 --- /dev/null +++ b/apps/fac/fsw/src/ECL_RollController.cpp @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + +#include "ECL_RollController.hpp" + + +ECL_RollController::ECL_RollController() +{ + +} + +ECL_RollController::~ECL_RollController() +{ + +} diff --git a/apps/fac/fsw/src/ECL_RollController.hpp b/apps/fac/fsw/src/ECL_RollController.hpp new file mode 100644 index 000000000..72bb38a3f --- /dev/null +++ b/apps/fac/fsw/src/ECL_RollController.hpp @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + +#ifndef ECL_ROLLCONTROLLER_HPP +#define ECL_ROLLCONTROLLER_HPP + +#include "ECL_Controller.hpp" + +class ECL_RollController : public ECL_Controller +{ +public: + ECL_RollController(); + ~ECL_RollController(); +}; + +#endif diff --git a/apps/fac/fsw/src/ECL_WheelController.cpp b/apps/fac/fsw/src/ECL_WheelController.cpp new file mode 100644 index 000000000..0586b1ae8 --- /dev/null +++ b/apps/fac/fsw/src/ECL_WheelController.cpp @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + +#include "ECL_WheelController.hpp" + + +ECL_WheelController::ECL_WheelController() +{ + +} + +ECL_WheelController::~ECL_WheelController() +{ + +} diff --git a/apps/fac/fsw/src/ECL_WheelController.hpp b/apps/fac/fsw/src/ECL_WheelController.hpp new file mode 100644 index 000000000..74aabb25e --- /dev/null +++ b/apps/fac/fsw/src/ECL_WheelController.hpp @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + +#ifndef ECL_WHEELCONTROLLER_HPP +#define ECL_WHEELCONTROLLER_HPP + +#include "ECL_Controller.hpp" + +class ECL_WheelController : public ECL_Controller +{ +public: + ECL_WheelController(); + ~ECL_WheelController(); +}; + +#endif diff --git a/apps/fac/fsw/src/ECL_YawController.cpp b/apps/fac/fsw/src/ECL_YawController.cpp new file mode 100644 index 000000000..729b517f3 --- /dev/null +++ b/apps/fac/fsw/src/ECL_YawController.cpp @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + +#include "ECL_YawController.hpp" + + +ECL_YawController::ECL_YawController() +{ + +} + +ECL_YawController::~ECL_YawController() +{ + +} diff --git a/apps/fac/fsw/src/ECL_YawController.hpp b/apps/fac/fsw/src/ECL_YawController.hpp new file mode 100644 index 000000000..95516bd5d --- /dev/null +++ b/apps/fac/fsw/src/ECL_YawController.hpp @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + +#ifndef ECL_YAWCONTROLLER_HPP +#define ECL_YAWCONTROLLER_HPP + +#include "ECL_Controller.hpp" + +class ECL_YawController : public ECL_Controller +{ +public: + ECL_YawController(); + ~ECL_YawController(); +}; + +#endif diff --git a/apps/fac/fsw/src/fac_app.cpp b/apps/fac/fsw/src/fac_app.cpp index 4afc5f1f6..78f34e8ab 100644 --- a/apps/fac/fsw/src/fac_app.cpp +++ b/apps/fac/fsw/src/fac_app.cpp @@ -35,61 +35,45 @@ #include #include -#include "fac_fac.hpp" +#include "fac_tbldefs.h" #include "fac_app.hpp" -#include "fac_cds_task.hpp" -#include "fac_cmds_task.hpp" +#include "fac_fac.hpp" +#include "fac_data_utils.hpp" +#include "fac_cmds_utils.hpp" #include "fac_msg.h" #include "fac_version.h" #include +FAC_AppData_t FAC_AppData; -FAC_AppData_t FAC_MainAppData; - -FixedwingAttitudeControl *pFAC = NULL; - - -extern FAC_CdsTaskData_t FAC_CdsTaskData; -extern FAC_CmdsTaskData_t FAC_CmdsTaskData; +FixedwingAttitudeControl objFAC; +AppCommandProcess objCmds; +AppDataProcess objData; FixedwingAttitudeControl::FixedwingAttitudeControl() { - FAC_MainAppData.MainAppId = 0, - FAC_MainAppData.uiAppRunStatus = CFE_ES_APP_RUN; - FAC_MainAppData.bCdsTaskCreated = FALSE; - FAC_MainAppData.bCmdsTaskCreated = FALSE; - FAC_MainAppData.bAppAwaken = FALSE; -} + FAC_AppData.uiAppRunStatus = CFE_ES_APP_RUN; + FAC_AppData.bAppAwaken = FALSE; -FixedwingAttitudeControl::~FixedwingAttitudeControl() -{ - FAC_MainAppData.MainAppId = 0; - FAC_MainAppData.uiAppRunStatus = CFE_ES_APP_EXIT; - FAC_MainAppData.bCdsTaskCreated = FALSE; - FAC_MainAppData.bCmdsTaskCreated = FALSE; - FAC_MainAppData.bAppAwaken = FALSE; + FAC_AppData.ConfigTblHdl = 0; + FAC_AppData.ConfigTblPtr = NULL; } -void FixedwingAttitudeControl::SendOutData() +FixedwingAttitudeControl::~FixedwingAttitudeControl() { -#if 0 // check this - /* TODO: Add code to update output data, if needed, here. */ + FAC_AppData.uiAppRunStatus = CFE_ES_APP_EXIT; + FAC_AppData.bAppAwaken = FALSE; - CFE_SB_TimeStampMsg((CFE_SB_Msg_t*)&FAC_MainAppData.OutData); - int32 iStatus = CFE_SB_SendMsg((CFE_SB_Msg_t*)&FAC_MainAppData.OutData); - if (iStatus != CFE_SUCCESS) - { - /* TODO: Decide what to do if the send message fails. */ - } -#endif + FAC_AppData.ConfigTblHdl = 0; + FAC_AppData.ConfigTblPtr = NULL; } -boolean FixedwingAttitudeControl::VerifySchCmdLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen) +int32 FixedwingAttitudeControl::VerifySchCmdLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen) { - boolean bResult = TRUE; uint16 usMsgLen = 0; + int32 iStatus = CFE_SUCCESS; if (MsgPtr != NULL) { @@ -97,7 +81,7 @@ boolean FixedwingAttitudeControl::VerifySchCmdLength(CFE_SB_Msg_t *MsgPtr, uint1 if (usExpectedLen != usMsgLen) { - bResult = FALSE; + iStatus = FAC_ERR_MSG_LENGTH; CFE_SB_MsgId_t MsgId = CFE_SB_GetMsgId(MsgPtr); uint16 usCmdCode = CFE_SB_GetCmdCode(MsgPtr); @@ -109,11 +93,10 @@ boolean FixedwingAttitudeControl::VerifySchCmdLength(CFE_SB_Msg_t *MsgPtr, uint1 } else { - bResult = FALSE; - FAC_MainAppData.uiAppRunStatus = CFE_ES_APP_ERROR; + iStatus = FAC_ERR_INVALID_POINTER; } - return (bResult); + return (iStatus); } void FixedwingAttitudeControl::ReportHousekeeping() @@ -122,8 +105,8 @@ void FixedwingAttitudeControl::ReportHousekeeping() /* TODO: Add code to update housekeeping data, if needed, here. */ - CFE_SB_TimeStampMsg((CFE_SB_Msg_t*)&FAC_MainAppData.HkTlm); - iStatus = CFE_SB_SendMsg((CFE_SB_Msg_t*)&FAC_MainAppData.HkTlm); + CFE_SB_TimeStampMsg((CFE_SB_Msg_t*)&FAC_AppData.HkTlm); + iStatus = CFE_SB_SendMsg((CFE_SB_Msg_t*)&FAC_AppData.HkTlm); if (iStatus != CFE_SUCCESS) { /* TODO: Decide what to do if the send message fails. */ @@ -134,80 +117,61 @@ void FixedwingAttitudeControl::ReportHousekeeping() int32 FixedwingAttitudeControl::SendHkMsg(CFE_SB_Msg_t *MsgPtr, CFE_SB_MsgId_t MsgId) { int32 iStatus = CFE_SUCCESS; - boolean bResult = TRUE; - bResult = VerifySchCmdLength(MsgPtr, sizeof(FAC_NoArgCmd_t)); - if (bResult == TRUE) + iStatus = VerifySchCmdLength(MsgPtr, sizeof(FAC_NoArgCmd_t)); + if (iStatus == CFE_SUCCESS) { CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, "Recvd FAC_SEND_HK_MID msgId (0x%04X)", (unsigned short)MsgId); - if (FAC_MainAppData.bAppAwaken == TRUE) + if (FAC_AppData.bAppAwaken == TRUE) { ReportHousekeeping(); } } + else if (iStatus == FAC_ERR_INVALID_POINTER) + { + CFE_EVS_SendEvent(FAC_ERR_EID, CFE_EVS_ERROR, "Invalid Msg Pointer (0x%04X)", + (unsigned short)MsgId); + FAC_AppData.HkTlm.usCmdErrCnt++; + } else { - FAC_MainAppData.HkTlm.usCmdErrCnt++; + FAC_AppData.HkTlm.usCmdErrCnt++; } return (iStatus); } -int32 FixedwingAttitudeControl::WakeupMsg(CFE_SB_Msg_t *MsgPtr, CFE_SB_MsgId_t MsgId) +int32 FixedwingAttitudeControl::WakeupValidate(CFE_SB_Msg_t *MsgPtr, CFE_SB_MsgId_t MsgId) { int32 iStatus = CFE_SUCCESS; - boolean bResult = TRUE; - bResult = VerifySchCmdLength(MsgPtr, sizeof(FAC_NoArgCmd_t)); - if (bResult == TRUE) + iStatus = VerifySchCmdLength(MsgPtr, sizeof(FAC_NoArgCmd_t)); + if (iStatus == CFE_SUCCESS) { CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, "Recvd FAC_WAKEUP_MID msgId (0x%04X)", (unsigned short)MsgId); - if (FAC_MainAppData.bCdsTaskCreated == FALSE) - { - iStatus = CFE_ES_CreateChildTask(&FAC_CdsTaskData.CdsTaskId, - FAC_CDS_TASK_NAME, CdsTask, - NULL, CFE_ES_DEFAULT_STACK_SIZE, - FAC_CDS_TASK_PRIORITY, OS_FP_ENABLED); - if(iStatus != CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_ERR_EID, CFE_EVS_ERROR, - "Failed to create CDS Task (0x%08X)", - (unsigned int)iStatus); - FAC_MainAppData.uiAppRunStatus = CFE_ES_APP_ERROR; - } - FAC_MainAppData.bCdsTaskCreated = TRUE; - } - - if (FAC_MainAppData.bCmdsTaskCreated == FALSE) - { - iStatus = CFE_ES_CreateChildTask(&FAC_CmdsTaskData.CmdsTaskId, - FAC_CMDS_TASK_NAME, CmdsTask, - NULL, CFE_ES_DEFAULT_STACK_SIZE, - FAC_CMDS_TASK_PRIORITY, OS_FP_ENABLED); - if(iStatus != CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_ERR_EID, CFE_EVS_ERROR, - "Failed to create CMDS Task (0x%08X)", - (unsigned int)iStatus); - FAC_MainAppData.uiAppRunStatus = CFE_ES_APP_ERROR; - } - FAC_MainAppData.bCmdsTaskCreated = TRUE; - } - - FAC_MainAppData.bAppAwaken = TRUE; + FAC_AppData.bAppAwaken = TRUE; /* TODO: Add more code here to handle other things when app wakes up */ } + else if (iStatus == FAC_ERR_INVALID_POINTER) + { + CFE_EVS_SendEvent(FAC_ERR_EID, CFE_EVS_ERROR, "Invalid Msg Pointer (0x%04X)", + (unsigned short)MsgId); + FAC_AppData.HkTlm.usCmdErrCnt++; + goto WakeupMsg_Exit_Tag; + } else { - FAC_MainAppData.HkTlm.usCmdErrCnt++; + FAC_AppData.HkTlm.usCmdErrCnt++; + goto WakeupMsg_Exit_Tag; } +WakeupMsg_Exit_Tag: return (iStatus); } @@ -232,7 +196,13 @@ int32 FixedwingAttitudeControl::RcvSchMsg(int32 iBlocking) switch (MsgId) { case FAC_WAKEUP_MID: - iStatus = WakeupMsg(MsgPtr, MsgId); + iStatus = WakeupValidate(MsgPtr, MsgId); // can be removed later + if (iStatus != CFE_SUCCESS) + { + goto RcvSchMsg_Exit_Tag; + } + + iStatus = Execute(); if (iStatus != CFE_SUCCESS) { goto RcvSchMsg_Exit_Tag; @@ -240,7 +210,7 @@ int32 FixedwingAttitudeControl::RcvSchMsg(int32 iBlocking) /* The last thing to do at the end of this Wakeup cycle should be to * automatically publish new output. */ - SendOutData(); + objData.SendOutData(); break; case FAC_SEND_HK_MID: @@ -255,7 +225,7 @@ int32 FixedwingAttitudeControl::RcvSchMsg(int32 iBlocking) CFE_EVS_SendEvent(FAC_MSGID_ERR_EID, CFE_EVS_ERROR, "Recvd invalid SCH msgId (0x%04X)", (unsigned short)MsgId); - FAC_MainAppData.HkTlm.usCmdErrCnt++; + FAC_AppData.HkTlm.usCmdErrCnt++; break; } } @@ -282,26 +252,490 @@ int32 FixedwingAttitudeControl::RcvSchMsg(int32 iBlocking) CFE_EVS_SendEvent(FAC_PIPE_ERR_EID, CFE_EVS_ERROR, "SB pipe read error (0x%08X), app will exit", (unsigned int)iStatus); - FAC_MainAppData.uiAppRunStatus= CFE_ES_APP_ERROR; + FAC_AppData.uiAppRunStatus= CFE_ES_APP_ERROR; } RcvSchMsg_Exit_Tag: return (iStatus); } +int32 FixedwingAttitudeControl::Execute() +{ + int32 status = CFE_SUCCESS; + + +// static uint64_t last_run = 0; +// float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; +// last_run = hrt_absolute_time(); +// +// /* guard against too large deltaT's */ +// if (deltaT > 1.0f) { +// deltaT = 0.01f; +// } +// +// /* load local copies */ +// orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); +// +// /* get current rotation matrix and euler angles from control state quaternions */ +// math::Quaternion q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]); +// _R = q_att.to_dcm(); +// +// math::Vector<3> euler_angles; +// euler_angles = _R.to_euler(); +// _roll = euler_angles(0); +// _pitch = euler_angles(1); +// _yaw = euler_angles(2); +// +// if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) { +// /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode +// * +// * Since the VTOL airframe is initialized as a multicopter we need to +// * modify the estimated attitude for the fixed wing operation. +// * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around +// * the pitch axis compared to the neutral position of the vehicle in multicopter mode +// * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. +// * Additionally, in order to get the correct sign of the pitch, we need to multiply +// * the new x axis of the rotation matrix with -1 +// * +// * original: modified: +// * +// * Rxx Ryx Rzx -Rzx Ryx Rxx +// * Rxy Ryy Rzy -Rzy Ryy Rxy +// * Rxz Ryz Rzz -Rzz Ryz Rxz +// * */ +// math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix +// +// /* move z to x */ +// R_adapted(0, 0) = _R(0, 2); +// R_adapted(1, 0) = _R(1, 2); +// R_adapted(2, 0) = _R(2, 2); +// +// /* move x to z */ +// R_adapted(0, 2) = _R(0, 0); +// R_adapted(1, 2) = _R(1, 0); +// R_adapted(2, 2) = _R(2, 0); +// +// /* change direction of pitch (convert to right handed system) */ +// R_adapted(0, 0) = -R_adapted(0, 0); +// R_adapted(1, 0) = -R_adapted(1, 0); +// R_adapted(2, 0) = -R_adapted(2, 0); +// euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation +// +// /* fill in new attitude data */ +// _R = R_adapted; +// _roll = euler_angles(0); +// _pitch = euler_angles(1); +// _yaw = euler_angles(2); +// +// /* lastly, roll- and yawspeed have to be swaped */ +// float helper = _att.rollspeed; +// _att.rollspeed = -_att.yawspeed; +// _att.yawspeed = helper; +// } +// +// _sub_airspeed.update(); +// vehicle_setpoint_poll(); +// vehicle_control_mode_poll(); +// vehicle_manual_poll(); +// global_pos_poll(); +// vehicle_status_poll(); +// vehicle_land_detected_poll(); +// battery_status_poll(); +// +// // the position controller will not emit attitude setpoints in some modes +// // we need to make sure that this flag is reset +// _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; +// +// /* lock integrator until control is started */ +// bool lock_integrator = !(_vcontrol_mode.flag_control_rates_enabled && !_vehicle_status.is_rotary_wing); +// +// /* Simple handling of failsafe: deploy parachute if failsafe is on */ +// if (_vcontrol_mode.flag_control_termination_enabled) { +// _actuators_airframe.control[7] = 1.0f; +// //warnx("_actuators_airframe.control[1] = 1.0f;"); +// +// } else { +// _actuators_airframe.control[7] = 0.0f; +// //warnx("_actuators_airframe.control[1] = -1.0f;"); +// } +// +// /* if we are in rotary wing mode, do nothing */ +// if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) { +// continue; +// } +// +// /* default flaps to center */ +// float flap_control = 0.0f; +// +// /* map flaps by default to manual if valid */ +// if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled +// && fabsf(_parameters.flaps_scale) > 0.01f) { +// flap_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale; +// +// } else if (_vcontrol_mode.flag_control_auto_enabled +// && fabsf(_parameters.flaps_scale) > 0.01f) { +// flap_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; +// } +// +// // move the actual control value continuous with time, full flap travel in 1sec +// if (fabsf(_flaps_applied - flap_control) > 0.01f) { +// _flaps_applied += (_flaps_applied - flap_control) < 0 ? deltaT : -deltaT; +// +// } else { +// _flaps_applied = flap_control; +// } +// +// /* default flaperon to center */ +// float flaperon_control = 0.0f; +// +// /* map flaperons by default to manual if valid */ +// if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled +// && fabsf(_parameters.flaperon_scale) > 0.01f) { +// flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; +// +// } else if (_vcontrol_mode.flag_control_auto_enabled +// && fabsf(_parameters.flaperon_scale) > 0.01f) { +// flaperon_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; +// } +// +// // move the actual control value continuous with time, full flap travel in 1sec +// if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) { +// _flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? deltaT : -deltaT; +// +// } else { +// _flaperons_applied = flaperon_control; +// } +// +// // Check if we are in rattitude mode and the pilot is above the threshold on pitch +// if (_vcontrol_mode.flag_control_rattitude_enabled) { +// if (fabsf(_manual.y) > _parameters.rattitude_thres || +// fabsf(_manual.x) > _parameters.rattitude_thres) { +// _vcontrol_mode.flag_control_attitude_enabled = false; +// } +// } +// +// /* decide if in stabilized or full manual control */ +// if (_vcontrol_mode.flag_control_rates_enabled) { +// /* scale around tuning airspeed */ +// float airspeed; +// +// /* if airspeed is non-finite or not valid or if we are asked not to control it, we assume the normal average speed */ +// const bool airspeed_valid = PX4_ISFINITE(_sub_airspeed.get().indicated_airspeed_m_s) +// && ((_sub_airspeed.get().timestamp - hrt_absolute_time()) < 1e6); +// +// if (airspeed_valid) { +// /* prevent numerical drama by requiring 0.5 m/s minimal speed */ +// airspeed = math::max(0.5f, _sub_airspeed.get().indicated_airspeed_m_s); +// +// } else { +// airspeed = _parameters.airspeed_trim; +// perf_count(_nonfinite_input_perf); +// } +// +// /* +// * For scaling our actuators using anything less than the min (close to stall) +// * speed doesn't make any sense - its the strongest reasonable deflection we +// * want to do in flight and its the baseline a human pilot would choose. +// * +// * Forcing the scaling to this value allows reasonable handheld tests. +// */ +// float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : +// airspeed); +// +// /* Use min airspeed to calculate ground speed scaling region. +// * Don't scale below gspd_scaling_trim +// */ +// float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + +// _global_pos.vel_e * _global_pos.vel_e); +// float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); +// float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); +// +// // in STABILIZED mode we need to generate the attitude setpoint +// // from manual user inputs +// if (!_vcontrol_mode.flag_control_climb_rate_enabled && !_vcontrol_mode.flag_control_offboard_enabled) { +// _att_sp.timestamp = hrt_absolute_time(); +// _att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad; +// _att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max); +// _att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad; +// _att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max); +// _att_sp.yaw_body = 0.0f; +// _att_sp.thrust = _manual.z; +// +// Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); +// q.copyTo(_att_sp.q_d); +// _att_sp.q_d_valid = true; +// +// int instance; +// orb_publish_auto(_attitude_setpoint_id, &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT); +// } +// +// /* reset integrals where needed */ +// if (_att_sp.roll_reset_integral) { +// _roll_ctrl.reset_integrator(); +// } +// +// if (_att_sp.pitch_reset_integral) { +// _pitch_ctrl.reset_integrator(); +// } +// +// if (_att_sp.yaw_reset_integral) { +// _yaw_ctrl.reset_integrator(); +// _wheel_ctrl.reset_integrator(); +// } +// +// /* Reset integrators if the aircraft is on ground +// * or a multicopter (but not transitioning VTOL) +// */ +// if (_vehicle_land_detected.landed +// || (_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode)) { +// +// _roll_ctrl.reset_integrator(); +// _pitch_ctrl.reset_integrator(); +// _yaw_ctrl.reset_integrator(); +// _wheel_ctrl.reset_integrator(); +// } +// +// float roll_sp = _att_sp.roll_body; +// float pitch_sp = _att_sp.pitch_body; +// float yaw_sp = _att_sp.yaw_body; +// float throttle_sp = _att_sp.thrust; +// +// /* Prepare data for attitude controllers */ +// struct ECL_ControlData control_input = {}; +// control_input.roll = _roll; +// control_input.pitch = _pitch; +// control_input.yaw = _yaw; +// control_input.body_x_rate = _att.rollspeed; +// control_input.body_y_rate = _att.pitchspeed; +// control_input.body_z_rate = _att.yawspeed; +// control_input.roll_setpoint = roll_sp; +// control_input.pitch_setpoint = pitch_sp; +// control_input.yaw_setpoint = yaw_sp; +// control_input.airspeed_min = _parameters.airspeed_min; +// control_input.airspeed_max = _parameters.airspeed_max; +// control_input.airspeed = airspeed; +// control_input.scaler = airspeed_scaling; +// control_input.lock_integrator = lock_integrator; +// control_input.groundspeed = groundspeed; +// control_input.groundspeed_scaler = groundspeed_scaler; +// +// _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); +// +// /* Run attitude controllers */ +// if (_vcontrol_mode.flag_control_attitude_enabled) { +// if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { +// _roll_ctrl.control_attitude(control_input); +// _pitch_ctrl.control_attitude(control_input); +// _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude +// _wheel_ctrl.control_attitude(control_input); +// +// /* Update input data for rate controllers */ +// control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); +// control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); +// control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); +// +// /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ +// float roll_u = _roll_ctrl.control_euler_rate(control_input); +// _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : +// _parameters.trim_roll; +// +// if (!PX4_ISFINITE(roll_u)) { +// _roll_ctrl.reset_integrator(); +// perf_count(_nonfinite_output_perf); +// +// if (_debug && loop_counter % 10 == 0) { +// warnx("roll_u %.4f", (double)roll_u); +// } +// } +// +// float pitch_u = _pitch_ctrl.control_euler_rate(control_input); +// _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : +// _parameters.trim_pitch; +// +// if (!PX4_ISFINITE(pitch_u)) { +// _pitch_ctrl.reset_integrator(); +// perf_count(_nonfinite_output_perf); +// +// if (_debug && loop_counter % 10 == 0) { +// warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," +// " airspeed %.4f, airspeed_scaling %.4f," +// " roll_sp %.4f, pitch_sp %.4f," +// " _roll_ctrl.get_desired_rate() %.4f," +// " _pitch_ctrl.get_desired_rate() %.4f" +// " att_sp.roll_body %.4f", +// (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), +// (double)airspeed, (double)airspeed_scaling, +// (double)roll_sp, (double)pitch_sp, +// (double)_roll_ctrl.get_desired_rate(), +// (double)_pitch_ctrl.get_desired_rate(), +// (double)_att_sp.roll_body); +// } +// } +// +// float yaw_u = 0.0f; +// +// if (_parameters.w_en && _att_sp.fw_control_yaw) { +// yaw_u = _wheel_ctrl.control_bodyrate(control_input); +// +// } else { +// yaw_u = _yaw_ctrl.control_euler_rate(control_input); +// } +// +// _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : +// _parameters.trim_yaw; +// +// /* add in manual rudder control in manual modes */ +// if (_vcontrol_mode.flag_control_manual_enabled) { +// _actuators.control[actuator_controls_s::INDEX_YAW] += _manual.r; +// } +// +// if (!PX4_ISFINITE(yaw_u)) { +// _yaw_ctrl.reset_integrator(); +// _wheel_ctrl.reset_integrator(); +// perf_count(_nonfinite_output_perf); +// +// if (_debug && loop_counter % 10 == 0) { +// warnx("yaw_u %.4f", (double)yaw_u); +// } +// } +// +// /* throttle passed through if it is finite and if no engine failure was detected */ +// _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) && +// !(_vehicle_status.engine_failure || +// _vehicle_status.engine_failure_cmd)) ? +// throttle_sp : 0.0f; +// +// /* scale effort by battery status */ +// if (_parameters.bat_scale_en && _battery_status.scale > 0.0f && +// _actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) { +// _actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_status.scale; +// } +// +// +// if (!PX4_ISFINITE(throttle_sp)) { +// if (_debug && loop_counter % 10 == 0) { +// warnx("throttle_sp %.4f", (double)throttle_sp); +// } +// } +// +// } else { +// perf_count(_nonfinite_input_perf); +// +// if (_debug && loop_counter % 10 == 0) { +// warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); +// } +// } +// +// } else { +// // pure rate control +// _roll_ctrl.set_bodyrate_setpoint(_manual.y * _parameters.acro_max_x_rate_rad); +// _pitch_ctrl.set_bodyrate_setpoint(-_manual.x * _parameters.acro_max_y_rate_rad); +// _yaw_ctrl.set_bodyrate_setpoint(_manual.r * _parameters.acro_max_z_rate_rad); +// +// float roll_u = _roll_ctrl.control_bodyrate(control_input); +// _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : +// _parameters.trim_roll; +// +// float pitch_u = _pitch_ctrl.control_bodyrate(control_input); +// _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : +// _parameters.trim_pitch; +// +// float yaw_u = _yaw_ctrl.control_bodyrate(control_input); +// _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : +// _parameters.trim_yaw; +// +// _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) && +// //!(_vehicle_status.engine_failure || +// !_vehicle_status.engine_failure_cmd) ? +// throttle_sp : 0.0f; +// } +// +// /* +// * Lazily publish the rate setpoint (for analysis, the actuators are published below) +// * only once available +// */ +// _rates_sp.roll = _roll_ctrl.get_desired_bodyrate(); +// _rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate(); +// _rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate(); +// +// _rates_sp.timestamp = hrt_absolute_time(); +// +// if (_rate_sp_pub != nullptr) { +// /* publish the attitude rates setpoint */ +// orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); +// +// } else if (_rates_sp_id) { +// /* advertise the attitude rates setpoint */ +// _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); +// } +// +// } else { +// /* manual/direct control */ +// _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _parameters.man_roll_scale + _parameters.trim_roll; +// _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _parameters.man_pitch_scale + +// _parameters.trim_pitch; +// _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _parameters.man_yaw_scale + _parameters.trim_yaw; +// _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; +// } +// +// // Add feed-forward from roll control output to yaw control output +// // This can be used to counteract the adverse yaw effect when rolling the plane +// _actuators.control[actuator_controls_s::INDEX_YAW] += _parameters.roll_to_yaw_ff * math::constrain( +// _actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); +// +// _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; +// _actuators.control[5] = _manual.aux1; +// _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied; +// // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future +// _actuators.control[7] = _manual.aux3; +// +// /* lazily publish the setpoint only once available */ +// _actuators.timestamp = hrt_absolute_time(); +// _actuators.timestamp_sample = _att.timestamp; +// _actuators_airframe.timestamp = hrt_absolute_time(); +// _actuators_airframe.timestamp_sample = _att.timestamp; +// +// /* Only publish if any of the proper modes are enabled */ +// if (_vcontrol_mode.flag_control_rates_enabled || +// _vcontrol_mode.flag_control_attitude_enabled || +// _vcontrol_mode.flag_control_manual_enabled) { +// /* publish the actuator controls */ +// if (_actuators_0_pub != nullptr) { +// orb_publish(_actuators_id, _actuators_0_pub, &_actuators); +// +// } else if (_actuators_id) { +// _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); +// } +// +// if (_actuators_2_pub != nullptr) { +// /* publish the actuator controls*/ +// orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); +// +// } else { +// /* advertise and publish */ +// _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); +// } +// } + + + + + return status; +} + int32 FixedwingAttitudeControl::InitHk() { int32 iStatus = CFE_SUCCESS; /* Init housekeeping packet */ -// memset((void*)&FAC_MainAppData.HkTlm, 0x00, sizeof(FAC_MainAppData.HkTlm)); // check this - CFE_SB_InitMsg(&FAC_MainAppData.HkTlm, FAC_HK_TLM_MID, - sizeof(FAC_MainAppData.HkTlm), TRUE); +// memset((void*)&FAC_AppData.HkTlm, 0x00, sizeof(FAC_AppData.HkTlm)); // check this + CFE_SB_InitMsg(&FAC_AppData.HkTlm, FAC_HK_TLM_MID, + sizeof(FAC_AppData.HkTlm), TRUE); return (iStatus); } -int32 FixedwingAttitudeControl::InitPipe() +int32 FixedwingAttitudeControl::InitSchPipe() { int32 iStatus = CFE_SUCCESS; @@ -316,7 +750,7 @@ int32 FixedwingAttitudeControl::InitPipe() CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, "Sch Pipe failed to subscribe to FAC_WAKEUP_MID. (0x%08X)", (unsigned int)iStatus); - goto InitPipe_Exit_Tag; + goto InitSchPipe_Exit_Tag; } iStatus = CFE_SB_SubscribeEx(FAC_SEND_HK_MID, SchPipeId, CFE_SB_Default_Qos, @@ -326,7 +760,7 @@ int32 FixedwingAttitudeControl::InitPipe() CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, "CMD Pipe failed to subscribe to FAC_SEND_HK_MID. (0x%08X)", (unsigned int)iStatus); - goto InitPipe_Exit_Tag; + goto InitSchPipe_Exit_Tag; } } else @@ -334,10 +768,10 @@ int32 FixedwingAttitudeControl::InitPipe() CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, "Failed to create SCH pipe (0x%08X)", (unsigned int)iStatus); - goto InitPipe_Exit_Tag; + goto InitSchPipe_Exit_Tag; } -InitPipe_Exit_Tag: +InitSchPipe_Exit_Tag: return (iStatus); } @@ -385,13 +819,39 @@ int32 FixedwingAttitudeControl::InitEvent() return (iStatus); } +int32 FAC_RcvMsg() +{ + int32 iStatus = CFE_SUCCESS; + + iStatus = objFAC.RcvSchMsg(FAC_SCH_PIPE_PEND_TIME); + if (iStatus != CFE_SUCCESS) + { + goto RcvMsg_Exit_Tag; + } + + iStatus = objCmds.RcvCmdMsg(CFE_SB_POLL); + if (iStatus != CFE_SUCCESS) + { + goto RcvMsg_Exit_Tag; + } + + iStatus = objData.RcvDataMsg(CFE_SB_POLL); + if (iStatus != CFE_SUCCESS) + { + goto RcvMsg_Exit_Tag; + } + +RcvMsg_Exit_Tag: + return iStatus; +} + /* Startup steps to initialize (or restore from CDS) FAC data structures */ -int32 FixedwingAttitudeControl::InitApp() +int32 FAC_InitApp() { int32 iStatus = CFE_SUCCESS; int8 hasEvents = 0; - iStatus = InitEvent(); + iStatus = objFAC.InitEvent(); if (iStatus != CFE_SUCCESS) { CFE_ES_WriteToSysLog("FAC - Failed to init events (0x%08X)\n", @@ -403,16 +863,34 @@ int32 FixedwingAttitudeControl::InitApp() hasEvents = 1; } - iStatus = InitPipe(); + iStatus = objFAC.InitSchPipe(); + if (iStatus != CFE_SUCCESS) + { + CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, + "Failed to init Sch pipe (0x%08X)", + (unsigned int)iStatus); + goto InitApp_Exit_Tag; + } + + iStatus = objCmds.InitCmdsPipe(); + if (iStatus != CFE_SUCCESS) + { + CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, + "Failed to init Cmds pipe (0x%08X)", + (unsigned int)iStatus); + goto InitApp_Exit_Tag; + } + + iStatus = objData.InitDataPipe(); if (iStatus != CFE_SUCCESS) { CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to init pipes (0x%08X)", + "Failed to init Cds pipe (0x%08X)", (unsigned int)iStatus); goto InitApp_Exit_Tag; } - iStatus = InitHk(); + iStatus = objFAC.InitHk(); if (iStatus != CFE_SUCCESS) { CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, @@ -421,16 +899,14 @@ int32 FixedwingAttitudeControl::InitApp() goto InitApp_Exit_Tag; } -#if 0 - iStatus = OS_TaskInstallDeleteHandler(&FAC_CleanupCallback); // check this + iStatus = objData.InitTables(); if (iStatus != CFE_SUCCESS) { CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to init register cleanup callback (0x%08X)", + "Failed to init Tables (0x%08X)", (unsigned int)iStatus); goto InitApp_Exit_Tag; } -#endif InitApp_Exit_Tag: if (iStatus == CFE_SUCCESS) @@ -476,34 +952,32 @@ extern "C" void FAC_AppMain() /* Perform application initializations */ if (iStatus == CFE_SUCCESS) { - pFAC = new FixedwingAttitudeControl(); - - if (pFAC != NULL) + iStatus = FAC_InitApp(); // Constructors + if (iStatus == CFE_SUCCESS) { - iStatus = pFAC->InitApp(); // Constructors - if (iStatus == CFE_SUCCESS) - { - /* Do not perform performance monitoring on startup sync */ - CFE_ES_PerfLogExit(FAC_MAIN_TASK_PERF_ID); - CFE_ES_WaitForStartupSync(FAC_STARTUP_TIMEOUT_MSEC); - CFE_ES_PerfLogEntry(FAC_MAIN_TASK_PERF_ID); - } - else - { - FAC_MainAppData.uiAppRunStatus = CFE_ES_APP_ERROR; - } + /* Do not perform performance monitoring on startup sync */ + CFE_ES_PerfLogExit(FAC_MAIN_TASK_PERF_ID); + CFE_ES_WaitForStartupSync(FAC_STARTUP_TIMEOUT_MSEC); + CFE_ES_PerfLogEntry(FAC_MAIN_TASK_PERF_ID); } else { - FAC_MainAppData.uiAppRunStatus = CFE_ES_APP_ERROR; + FAC_AppData.uiAppRunStatus = CFE_ES_APP_ERROR; } } /* Application main loop */ - while (CFE_ES_RunLoop(&FAC_MainAppData.uiAppRunStatus) == TRUE) + while (CFE_ES_RunLoop(&FAC_AppData.uiAppRunStatus) == TRUE) { - iStatus = pFAC->RcvSchMsg(FAC_SCH_PIPE_PEND_TIME); - if (iStatus != CFE_SUCCESS) + iStatus = FAC_RcvMsg(); + if (iStatus == FAC_ERR_INVALID_POINTER) + { + CFE_EVS_SendEvent(FAC_ERR_EID, CFE_EVS_ERROR, + "Application Fatal Error(Invalid pointer)"); + FAC_AppData.uiAppRunStatus = CFE_ES_APP_ERROR; + goto AppMain_Exit_Tag; + } + else if (iStatus != CFE_SUCCESS) { /* TODO: Decide what to do for other return values in FAC_RcvMsg(). */ } @@ -513,41 +987,9 @@ extern "C" void FAC_AppMain() ** Depends on the nature of the application, the frequency of update ** and save can be more or less independently. */ -#if 0 // check this - FAC_UpdateCdsTbl(); - FAC_SaveCdsTbl(); -#endif } AppMain_Exit_Tag: - - // Destructors - if (FAC_MainAppData.bCdsTaskCreated == TRUE) - { - if (FAC_CdsTaskData.pCds != NULL) - { - delete FAC_CdsTaskData.pCds; - FAC_CdsTaskData.pCds = NULL; - } - CFE_ES_DeleteChildTask(FAC_CdsTaskData.CdsTaskId); // check this - } - - if (FAC_MainAppData.bCmdsTaskCreated == TRUE) - { - if (FAC_CmdsTaskData.pCmds != NULL) - { - delete FAC_CmdsTaskData.pCmds; - FAC_CmdsTaskData.pCmds = NULL; - } - CFE_ES_DeleteChildTask(FAC_CmdsTaskData.CmdsTaskId); // check this - } - - if (pFAC != NULL) - { - delete pFAC; - pFAC = NULL; - } - CFE_ES_WriteToSysLog("FAC - Error detected. App will exit (0x%08X)\n", (unsigned int)iStatus); @@ -555,5 +997,5 @@ extern "C" void FAC_AppMain() CFE_ES_PerfLogExit(FAC_MAIN_TASK_PERF_ID); /* Exit the application */ - CFE_ES_ExitApp(FAC_MainAppData.uiAppRunStatus); + CFE_ES_ExitApp(FAC_AppData.uiAppRunStatus); } diff --git a/apps/fac/fsw/src/fac_app.hpp b/apps/fac/fsw/src/fac_app.hpp index fa70b1703..8c3a8ef7a 100644 --- a/apps/fac/fsw/src/fac_app.hpp +++ b/apps/fac/fsw/src/fac_app.hpp @@ -44,8 +44,6 @@ #include "fac_msgids.h" #include "fac_msg.h" #include "fac_events.h" -#include "fac_config_utils.hpp" -#include "fac_cds_utils.hpp" #define FAC_TIMEOUT_MSEC (1000) @@ -53,13 +51,14 @@ typedef struct { - uint32 MainAppId; uint32 uiAppRunStatus; - boolean bCdsTaskCreated; - boolean bCmdsTaskCreated; boolean bAppAwaken; FAC_HkTlm_t HkTlm; + + CFE_TBL_Handle_t ConfigTblHdl; + + FAC_ConfigTbl_t *ConfigTblPtr; } FAC_AppData_t; diff --git a/apps/fac/fsw/src/fac_cds_task.cpp b/apps/fac/fsw/src/fac_cds_task.cpp deleted file mode 100644 index 714695c0d..000000000 --- a/apps/fac/fsw/src/fac_cds_task.cpp +++ /dev/null @@ -1,478 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name Windhover Labs nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - *****************************************************************************/ - -#include -#include - -#include "fac_app.hpp" -#include "fac_cds_task.hpp" -#include "fac_msgids.h" - -#include - - -const FAC_CDS_DataGrpTbl_t FAC_Cds_GrpTbl[] = -{ - { PX4_ACTUATOR_ARMED_MID, FAC_GROUP_NONE }, - { PX4_ACTUATOR_CONTROLS_0_MID, FAC_GROUP_NONE }, - { PX4_ACTUATOR_CONTROLS_1_MID, FAC_GROUP_NONE }, - { PX4_ACTUATOR_CONTROLS_2_MID, FAC_GROUP_NONE }, - { PX4_ACTUATOR_CONTROLS_3_MID, FAC_GROUP_NONE }, - { PX4_ACTUATOR_DIRECT_MID, FAC_GROUP_NONE }, - { PX4_ACTUATOR_OUTPUTS_MID, FAC_GROUP_NONE }, - - { PX4_ADC_REPORT_MID, FAC_GROUP_NONE }, - { PX4_AIRSPEED_MID, FAC_GROUP_NONE }, - { PX4_ATT_POS_MOCAP_MID, FAC_GROUP_NONE }, - { PX4_BATTERY_STATUS_MID, FAC_GROUP_NONE }, - { PX4_CAMERA_TRIGGER_MID, FAC_GROUP_NONE }, - { PX4_COMMANDER_STATE_MID, FAC_GROUP_NONE }, - { PX4_CONTROL_STATE_MID, FAC_GROUP_NONE }, - { PX4_CPULOAD_MID, FAC_GROUP_NONE }, - { PX4_DEBUG_KEY_VALUE_MID, FAC_GROUP_NONE }, - { PX4_DIFFERENTIAL_PRESSURE_MID, FAC_GROUP_NONE }, - { PX4_DISTANCE_SENSOR_MID, FAC_GROUP_NONE }, - - { PX4_FW_POS_CTRL_STATUS_MID, FAC_GROUP_NONE }, - { PX4_FW_VIRTUAL_ATTITUDE_SETPOINT_MID, FAC_GROUP_NONE }, - { PX4_FW_VIRTUAL_RATES_SETPOINT_MID, FAC_GROUP_NONE }, - - { PX4_EKF2_INNOVATIONS_MID, FAC_GROUP_NONE }, - { PX4_EKF2_REPLAY_MID, FAC_GROUP_NONE }, - - { PX4_ESC_REPORT_MID, FAC_GROUP_NONE }, - { PX4_ESC_STATUS_MID, FAC_GROUP_NONE }, - - { PX4_ESTIMATOR_STATUS_MID, FAC_GROUP_NONE }, - - { PX4_FENCE_MID, FAC_GROUP_NONE }, - { PX4_FENCE_VERTEX_MID, FAC_GROUP_NONE }, - - { PX4_FILTERED_BOTTOM_FLOW_MID, FAC_GROUP_NONE }, - { PX4_FOLLOW_TARGET_MID, FAC_GROUP_NONE }, - { PX4_GEOFENCE_RESULT_MID, FAC_GROUP_NONE }, - - { PX4_GPS_DUMP_MID, FAC_GROUP_NONE }, - { PX4_GPS_INJECT_DATA_MID, FAC_GROUP_NONE }, - - { PX4_HIL_SENSOR_MID, FAC_GROUP_NONE }, - { PX4_HOME_POSITION_MID, FAC_GROUP_NONE }, - { PX4_INPUT_RC_MID, FAC_GROUP_NONE }, - { PX4_LED_CONTROL_MID, FAC_GROUP_NONE }, - { PX4_LOG_MESSAGE_MID, FAC_GROUP_NONE }, - { PX4_MANUAL_CONTROL_SETPOINT_MID, FAC_GROUP_NONE }, - { PX4_MAVLINK_LOG_MID, FAC_GROUP_NONE }, - - { PX4_MC_ATT_CTRL_STATUS_MID, FAC_GROUP_NONE }, - { PX4_MC_VIRTUAL_ATTITUDE_SETPOINT_MID, FAC_GROUP_NONE }, - { PX4_MC_VIRTUAL_RATES_SETPOINT_MID, FAC_GROUP_NONE }, - - { PX4_MISSION_MID, FAC_GROUP_NONE }, - { PX4_MISSION_RESULT_MID, FAC_GROUP_NONE }, - - { PX4_MULTIROTOR_MOTOR_LIMITS_MID, FAC_GROUP_NONE }, - { PX4_OFFBOARD_CONTROL_MODE_MID, FAC_GROUP_NONE }, - { PX4_OPTICAL_FLOW_MID, FAC_GROUP_NONE }, - { PX4_OUTPUT_PWM_MID, FAC_GROUP_NONE }, - { PX4_PARAMETER_UPDATE_MID, FAC_GROUP_NONE }, - - { PX4_POSITION_SETPOINT_MID, FAC_GROUP_NONE }, - { PX4_POSITION_SETPOINT_TRIPLET_MID, FAC_GROUP_NONE }, - - { PX4_PWM_INPUT_MID, FAC_GROUP_NONE }, - { PX4_QSHELL_REQ_MID, FAC_GROUP_NONE }, - - { PX4_RC_CHANNELS_MID, FAC_GROUP_NONE }, - { PX4_RC_PARAMETER_MAP_MID, FAC_GROUP_NONE }, - - { PX4_SAFETY_MID, FAC_GROUP_NONE }, - { PX4_SATELLITE_INFO_MID, FAC_GROUP_NONE }, - - { PX4_SENSOR_ACCEL_MID, FAC_GROUP_NONE }, - { PX4_SENSOR_BARO_MID, FAC_GROUP_NONE }, - { PX4_SENSOR_COMBINED_MID, FAC_GROUP_NONE }, - { PX4_SENSOR_GYRO_MID, FAC_GROUP_NONE }, - { PX4_SENSOR_MAG_MID, FAC_GROUP_NONE }, - - { PX4_SERVORAIL_STATUS_MID, FAC_GROUP_NONE }, - { PX4_SUBSYSTEM_INFO_MID, FAC_GROUP_NONE }, - { PX4_SYSTEM_POWER_MID, FAC_GROUP_NONE }, - { PX4_TECS_STATUS_MID, FAC_GROUP_NONE }, - { PX4_TELEMETRY_STATUS_MID, FAC_GROUP_NONE }, - { PX4_TEST_MOTOR_MID, FAC_GROUP_NONE }, - { PX4_TIME_OFFSET_MID, FAC_GROUP_NONE }, - { PX4_TRANSPONDER_REPORT_MID, FAC_GROUP_NONE }, - - { PX4_UAVCAN_PARAMETER_REQUEST_MID, FAC_GROUP_NONE }, - { PX4_UAVCAN_PARAMETER_VALUE_MID, FAC_GROUP_NONE }, - - { PX4_VEHICLE_ATTITUDE_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_ATTITUDE_SETPOINT_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_COMMAND_ACK_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_COMMAND_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_CONTROL_MODE_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_FORCE_SETPOINT_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_GLOBAL_POSITION_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_GLOBAL_VELOCITY_SETPOINT_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_GPS_POSITION_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_LAND_DETECTED_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_LOCAL_POSITION_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_LOCAL_POSITION_SETPOINT_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_RATES_SETPOINT_MID, FAC_GROUP_NONE }, - { PX4_VEHICLE_STATUS_MID, FAC_GROUP_NONE }, - - { PX4_VISION_POSITION_ESTIMATE_MID, FAC_GROUP_NONE }, - { PX4_VTOL_VEHICLE_STATUS_MID, FAC_GROUP_NONE }, - { PX4_WIND_ESTIMATE_MID, FAC_GROUP_NONE }, - { PX4_SENSOR_CORRECTION_MID, FAC_GROUP_NONE } -}; - -const FAC_CDS_PX4HandlerTblRec_t FAC_Cds_PX4MsgHandlerTbl[] = -{ -#if 0 - {FAC_GROUP_NONE, sizeof(FAC_GrpNone_t), FAC_PX4GrpNoneFunc}, - {FAC_GROUP_Actuator, sizeof(FAC_GrpActuator_t), FAC_PX4GrpActuatorFunc}, - {FAC_GROUP_Fw, sizeof(FAC_GrpFw_t), FAC_PX4GrpFwFunc}, - {FAC_GROUP_Vehicle, sizeof(FAC_GrpVehicle_t), FAC_PX4GrpVehicleFunc}, - /* .... */ - {FAC_GROUP_Sensor, sizeof(FAC_GrpSensor_t), FAC_PX4GrpSensorFunc} -#endif -}; - -FAC_CdsTaskData_t FAC_CdsTaskData; - -extern FAC_AppData_t FAC_MainAppData; - -CriticalDataStorage::CriticalDataStorage() -{ - FAC_CdsTaskData.CdsTaskId = 0; - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_RUN; - FAC_CdsTaskData.pCds = NULL; - - FAC_CdsTaskData.ConfigTblPtr = NULL; -} - -CriticalDataStorage::~CriticalDataStorage() -{ - FAC_CdsTaskData.CdsTaskId = 0; - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_EXIT; - - FAC_CdsTaskData.ConfigTblPtr = NULL; -} - -int32 CriticalDataStorage::GetCdsGrpId(CFE_SB_MsgId_t DataMsgId) -{ - int32 GrpId = 0; - int32 cds_len = ( sizeof(FAC_Cds_GrpTbl) / sizeof(FAC_CDS_DataGrpTbl_t) ); // check real size - - for (int i = 0; i < cds_len; i++) - { - if (FAC_Cds_GrpTbl[i].msgId == DataMsgId) - { - GrpId = FAC_Cds_GrpTbl[i].cdsGrpId; - break; - } - } - - return GrpId; -} - -int32 CriticalDataStorage::RcvDataMsg(int32 iBlocking) -{ - int32 CdsGrpId = 0; - int32 iStatus = CFE_SUCCESS; - CFE_SB_Msg_t *DataMsgPtr = NULL; - CFE_SB_MsgId_t DataMsgId; - - while (FAC_CdsTaskData.uiCdsRunStatus == FAC_TASK_RUN) - { - iStatus = CFE_SB_RcvMsg(&DataMsgPtr, DataPipeId, CFE_SB_POLL); // check CFE_SB_POLL - if (iStatus == CFE_SUCCESS) - { - DataMsgId = CFE_SB_GetMsgId(DataMsgPtr); - CdsGrpId = GetCdsGrpId(DataMsgId); - if (CdsGrpId > 0) - { - // Find the handler and execute it - } - else - { - CFE_EVS_SendEvent(FAC_MSGID_ERR_EID, CFE_EVS_ERROR, - "Recvd invalid data msgId (0x%04X)", - (unsigned short)DataMsgId); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; // Decide what to do: continue? - goto RcvDataMsg_Exit_Tag; - } - } - else if (iStatus == CFE_SB_NO_MESSAGE) - { - iStatus = CFE_SUCCESS; - } - else - { - CFE_EVS_SendEvent(FAC_PIPE_ERR_EID, CFE_EVS_ERROR, - "Data pipe read error (0x%08X)", (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto RcvDataMsg_Exit_Tag; - } - - iStatus = FAC_AcquireConfigPointers(); - if (iStatus != CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_CONFIG_TABLE_ERR_EID, CFE_EVS_ERROR, - "AcquireConfigPointers error (0x%08X)", - (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto RcvDataMsg_Exit_Tag; - } - else - { -#if 0 - CFE_EVS_SendEvent(FAC_CONFIG_TABLE_INF_EID, CFE_EVS_INFORMATION, - "Succeeded to AcquireConfigPointers ********** (0x%08X)", - (unsigned int)iStatus); -#endif - } - } - -RcvDataMsg_Exit_Tag: - return iStatus; -} - -boolean CriticalDataStorage::VerifyCdsMsgLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen) -{ - boolean bResult = TRUE; - uint16 usMsgLen = 0; - - if (MsgPtr != NULL) - { - usMsgLen = CFE_SB_GetTotalMsgLength(MsgPtr); - - if (usExpectedLen != usMsgLen) - { - bResult = FALSE; - CFE_SB_MsgId_t MsgId = CFE_SB_GetMsgId(MsgPtr); - uint16 usCmdCode = CFE_SB_GetCmdCode(MsgPtr); - - CFE_EVS_SendEvent(FAC_MSGLEN_ERR_EID, CFE_EVS_ERROR, - "Rcvd invalid msgLen: msgId=0x%08X, cmdCode=%d, " - "msgLen=%d, expectedLen=%d", MsgId, usCmdCode, - usMsgLen, usExpectedLen); - } -// Add more to check - } - - return (bResult); -} - -int32 CriticalDataStorage::InitData() -{ - int32 iStatus = CFE_SUCCESS; - - /* Init input data */ - memset((void*)&InData, 0x00, sizeof(InData)); - - /* Init output data */ -// memset((void*)&OutData, 0x00, sizeof(OutData)); // Check for memset - /* memset entire OutData length and set the stream ID and length and - * restore sequence count if not TRUE - */ -// check cfe function for length - CFE_SB_InitMsg(&OutData, FAC_OUT_DATA_MID, sizeof(OutData), TRUE); // check length - - return (iStatus); -} - -int32 CriticalDataStorage::InitPipe() -{ - int32 iStatus = CFE_SUCCESS; - int32 cds_len = ( sizeof(FAC_Cds_GrpTbl) / sizeof(FAC_CDS_DataGrpTbl_t) ); // check real size - - iStatus = CFE_SB_CreatePipe(&DataPipeId, FAC_DATA_PIPE_DEPTH, FAC_DATA_PIPE_NAME); - if (iStatus == CFE_SUCCESS) - { - for ( int i = 0; i < cds_len; i++) - { - iStatus = CFE_SB_SubscribeEx(FAC_Cds_GrpTbl[i].msgId, DataPipeId, - CFE_SB_Default_Qos, FAC_DATA_PIPE_RESERVED); - if (iStatus != CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to subscribe to MID:0x%04X (0x%08X)", - FAC_Cds_GrpTbl[i].msgId, (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto InitPipe_Exit_Tag; - } - } - } - else - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to create Data pipe (0x%08X)", - (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto InitPipe_Exit_Tag; - } - -InitPipe_Exit_Tag: - return iStatus; -} - -int32 CriticalDataStorage::InitCdsTask() -{ - int32 iStatus = CFE_SUCCESS; - - iStatus = InitPipe(); - if (iStatus != CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to Init pipe (0x%08X)", (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto InitCdsTask_Exit_Tag; - } - - iStatus = InitData(); - if (iStatus != CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to initialize Data (0x%08X)", - (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto InitCdsTask_Exit_Tag; - } - - iStatus = FAC_InitConfigTbl(); - if (iStatus != CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to init config tables (0x%08X)", - (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto InitCdsTask_Exit_Tag; - } - - iStatus = FAC_InitCdsTbl(); - if (iStatus != CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to init CDS table (0x%08X)", - (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto InitCdsTask_Exit_Tag; - } - -InitCdsTask_Exit_Tag: - return iStatus; -} - -void CdsTask() -{ - int32 iStatus = CFE_SUCCESS; - CFE_ES_TaskInfo_t CdsTaskInfo; - - iStatus = CFE_ES_RegisterChildTask(); - if (iStatus != CFE_SUCCESS) - { - CFE_ES_WriteToSysLog("FAC - Failed to register the child task (0x%08X)\n", - (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto CdsTask_Exit_Tag; - } - - // Shoud this task wait for startup sync again?(done in the main apptask) - - iStatus = CFE_ES_GetTaskInfo(&CdsTaskInfo, FAC_CdsTaskData.CdsTaskId); //check taskId and appId - if (iStatus != CFE_SUCCESS) - { - CFE_ES_WriteToSysLog("FAC - Failed to Get child task info(0x%08X)\n", - (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto CdsTask_Exit_Tag; - } - else - { - FAC_MainAppData.MainAppId = CdsTaskInfo.AppId; - CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, - "Cds TaskInfo: TaskID: 0x%08X, TaskName: %s, AppId: 0x%08X, AppName:%s (0x%08X)\n", - (unsigned int)CdsTaskInfo.TaskId, CdsTaskInfo.TaskName, - (unsigned int)CdsTaskInfo.AppId, CdsTaskInfo.AppName, (unsigned int)iStatus); - } - - CFE_ES_IncrementTaskCounter(); - - // Start CDS task Performance Log entry? - - FAC_CdsTaskData.pCds = new CriticalDataStorage(); - if (FAC_CdsTaskData.pCds != NULL) - { - // Check performance monitoring - - iStatus = FAC_CdsTaskData.pCds->InitCdsTask(); - if (iStatus != CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to init cds task (0x%08X)", - (unsigned int)iStatus); - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto CdsTask_Exit_Tag; - } - } - else - { - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto CdsTask_Exit_Tag; - } - - if (FAC_CdsTaskData.uiCdsRunStatus == FAC_TASK_RUN) - { - iStatus = FAC_CdsTaskData.pCds->RcvDataMsg(FAC_DATA_PIPE_PEND_TIME); - if (iStatus != CFE_SUCCESS) - { - FAC_CdsTaskData.uiCdsRunStatus = FAC_TASK_ERROR; - goto CdsTask_Exit_Tag; - } - } - -CdsTask_Exit_Tag: - if (FAC_CdsTaskData.pCds != NULL) - { - delete FAC_CdsTaskData.pCds; - FAC_CdsTaskData.pCds = NULL; - } - CFE_ES_WriteToSysLog("FAC - CdsTask: Error detected. Task will exit (0x%08X)\n", - (unsigned int)iStatus); - CFE_ES_ExitChildTask(); - FAC_MainAppData.uiAppRunStatus = CFE_ES_APP_EXIT; - - return; -} diff --git a/apps/fac/fsw/src/fac_cds_utils.cpp b/apps/fac/fsw/src/fac_cds_utils.cpp deleted file mode 100644 index a46816295..000000000 --- a/apps/fac/fsw/src/fac_cds_utils.cpp +++ /dev/null @@ -1,109 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name Windhover Labs nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - *****************************************************************************/ - -#include -#include "fac_app.hpp" -#include "fac_cds_task.hpp" -#include "fac_cds_utils.hpp" - -extern FAC_CdsTaskData_t FAC_CdsTaskData; - - -int32 FAC_InitCdsTbl() -{ - int32 iStatus = CFE_SUCCESS; - int32 iResetType = 0; - uint32 uiResetSubType = 0; - - /* Only clear CDS table when it's a PowerOn reset, and not a Processor reset */ - iResetType = CFE_ES_GetResetType(&uiResetSubType); - if (iResetType == CFE_ES_POWERON_RESET) - { - memset((void*)&FAC_CdsTaskData.CdsTbl, 0x00, sizeof(FAC_CdsTbl_t)); - } - - /* Register and manage CDS table */ - iStatus = CFE_ES_RegisterCDS(&FAC_CdsTaskData.CdsTblHdl, sizeof(FAC_CdsTbl_t), - FAC_CDS_TABLENAME); - if (iStatus == CFE_SUCCESS) - { - /* Setup initial content of CDS table */ - iStatus = CFE_ES_CopyToCDS(FAC_CdsTaskData.CdsTblHdl, &FAC_CdsTaskData.CdsTbl); - if (iStatus == CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_CDS_INF_EID, CFE_EVS_INFORMATION, - "Successfully setup CDS"); - } - else - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to setup CDS"); - } - } - else if (iStatus == CFE_ES_CDS_ALREADY_EXISTS) - { - /* If one already exists, get a copy of its current content */ - memset((void*)&FAC_CdsTaskData.CdsTbl, 0x00, sizeof(FAC_CdsTbl_t)); - iStatus = CFE_ES_RestoreFromCDS(&FAC_CdsTaskData.CdsTbl, FAC_CdsTaskData.CdsTblHdl); - if (iStatus == CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_CDS_INF_EID, CFE_EVS_INFORMATION, - "Successfully restored data from CDS"); - } - else - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to restore data from CDS"); - memset((void*)&FAC_CdsTaskData.CdsTbl, 0x00, sizeof(FAC_CdsTbl_t)); - } - } - else - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to create CDS (0x%08X)", (unsigned int)iStatus); - } - - return (iStatus); -} - -void FAC_UpdateCdsTbl() -{ - /* TODO: Add code to update values in CDS table here */ -} - -void FAC_SaveCdsTbl() -{ - /* TODO This return value is not checked. Developer should decide what to do here - in case of failure or should add a return value for higher-level logic to handle. */ - CFE_ES_CopyToCDS(FAC_CdsTaskData.CdsTblHdl, &FAC_CdsTaskData.CdsTbl); -} diff --git a/apps/fac/fsw/src/fac_cmds_task.cpp b/apps/fac/fsw/src/fac_cmds_utils.cpp similarity index 54% rename from apps/fac/fsw/src/fac_cmds_task.cpp rename to apps/fac/fsw/src/fac_cmds_utils.cpp index 8e8261787..034e6490f 100644 --- a/apps/fac/fsw/src/fac_cmds_task.cpp +++ b/apps/fac/fsw/src/fac_cmds_utils.cpp @@ -34,32 +34,27 @@ #include #include +#include "fac_tbldefs.h" #include "fac_app.hpp" -#include "fac_cmds_task.hpp" +#include "fac_cmds_utils.hpp" #include "fac_version.h" -extern FAC_AppData_t FAC_MainAppData; +extern FAC_AppData_t FAC_AppData; -FAC_CmdsTaskData_t FAC_CmdsTaskData; AppCommandProcess::AppCommandProcess() { - FAC_CmdsTaskData.CmdsTaskId = 0; - FAC_CmdsTaskData.uiCmdsRunStatus = FAC_TASK_RUN; - FAC_CmdsTaskData.pCmds = NULL; } AppCommandProcess::~AppCommandProcess() { - FAC_CmdsTaskData.CmdsTaskId = 0; - FAC_CmdsTaskData.uiCmdsRunStatus = FAC_TASK_EXIT; } -boolean AppCommandProcess::VerifyCmdLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen) +int32 AppCommandProcess::VerifyCmdLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen) { - boolean bResult = TRUE; uint16 usMsgLen = 0; + int32 iStatus = CFE_SUCCESS; if (MsgPtr != NULL) { @@ -67,7 +62,7 @@ boolean AppCommandProcess::VerifyCmdLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpect if (usExpectedLen != usMsgLen) { - bResult = FALSE; + iStatus = FAC_ERR_MSG_LENGTH; CFE_SB_MsgId_t MsgId = CFE_SB_GetMsgId(MsgPtr); uint16 usCmdCode = CFE_SB_GetCmdCode(MsgPtr); @@ -78,15 +73,18 @@ boolean AppCommandProcess::VerifyCmdLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpect } // Add more to check } + else + { + iStatus = FAC_ERR_INVALID_POINTER; + } - return (bResult); + return (iStatus); } int32 AppCommandProcess::ProcessNewAppCmds(CFE_SB_Msg_t *MsgPtr) { int32 iStatus = CFE_SUCCESS; uint32 uiCmdCode = 0; - boolean bResult = TRUE; if (MsgPtr != NULL) { @@ -94,40 +92,54 @@ int32 AppCommandProcess::ProcessNewAppCmds(CFE_SB_Msg_t *MsgPtr) switch (uiCmdCode) { case FAC_NOOP_CC: - bResult = VerifyCmdLength(MsgPtr, sizeof(FAC_NoArgCmd_t)); - if (bResult == TRUE) + iStatus = VerifyCmdLength(MsgPtr, sizeof(FAC_NoArgCmd_t)); + if (iStatus == CFE_SUCCESS) { - FAC_MainAppData.HkTlm.usCmdCnt++; CFE_EVS_SendEvent(FAC_CMD_INF_EID, CFE_EVS_INFORMATION, "Recvd NOOP cmd (%u), Version %d.%d.%d.%d", (unsigned int)uiCmdCode, FAC_MAJOR_VERSION, FAC_MINOR_VERSION, FAC_REVISION, FAC_MISSION_REV); + FAC_AppData.HkTlm.usCmdCnt++; + } + else if (iStatus == FAC_ERR_INVALID_POINTER) + { + CFE_EVS_SendEvent(FAC_ERR_EID, CFE_EVS_ERROR, + "Invalid Cmd Msg Pointer (0x%08X)", + (unsigned int)iStatus); + FAC_AppData.HkTlm.usCmdErrCnt++; } else { - FAC_MainAppData.HkTlm.usCmdErrCnt++; + FAC_AppData.HkTlm.usCmdErrCnt++; } break; case FAC_RESET_CC: - bResult = VerifyCmdLength(MsgPtr, sizeof(FAC_NoArgCmd_t)); - if (bResult == TRUE) + iStatus = VerifyCmdLength(MsgPtr, sizeof(FAC_NoArgCmd_t)); + if (iStatus == CFE_SUCCESS) { - FAC_MainAppData.HkTlm.usCmdCnt = 0; - FAC_MainAppData.HkTlm.usCmdErrCnt = 0; + FAC_AppData.HkTlm.usCmdCnt = 0; + FAC_AppData.HkTlm.usCmdErrCnt = 0; CFE_EVS_SendEvent(FAC_CMD_INF_EID, CFE_EVS_INFORMATION, "Recvd RESET cmd (%u)", (unsigned int)uiCmdCode); } + else if (iStatus == FAC_ERR_INVALID_POINTER) + { + CFE_EVS_SendEvent(FAC_ERR_EID, CFE_EVS_ERROR, + "Invalid Cmd Msg Pointer (0x%08X)", + (unsigned int)iStatus); + FAC_AppData.HkTlm.usCmdErrCnt++; + } else { - FAC_MainAppData.HkTlm.usCmdErrCnt++; + FAC_AppData.HkTlm.usCmdErrCnt++; } break; /* TODO: Add code to process the rest of the FAC commands here */ default: - FAC_MainAppData.HkTlm.usCmdErrCnt++; + FAC_AppData.HkTlm.usCmdErrCnt++; CFE_EVS_SendEvent(FAC_MSGID_ERR_EID, CFE_EVS_ERROR, "Recvd invalid cmdId (%u)", (unsigned int)uiCmdCode); break; @@ -144,26 +156,25 @@ int32 AppCommandProcess::ProcessNewAppCmds(CFE_SB_Msg_t *MsgPtr) int32 AppCommandProcess::RcvCmdMsg(int32 iBlocking) { int32 iStatus = CFE_SUCCESS; - CFE_SB_Msg_t* MsgPtr=NULL; + CFE_SB_Msg_t* MsgPtr = NULL; CFE_SB_MsgId_t MsgId; /* Process command messages till the pipe is empty */ - while (FAC_CmdsTaskData.uiCmdsRunStatus == FAC_TASK_RUN) + iStatus = CFE_SB_RcvMsg(&MsgPtr, CmdPipeId, iBlocking); + if(iStatus == CFE_SUCCESS) { - iStatus = CFE_SB_RcvMsg(&MsgPtr, CmdPipeId, CFE_SB_POLL); - if(iStatus == CFE_SUCCESS) + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, // check display level + "RcvCmdMsg:Receivd message (0x%08X)", + (unsigned int)iStatus); + if (FAC_AppData.bAppAwaken == TRUE) { - CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, // check display level - "RcvCmdMsg:Receivd message (0x%08X)", - (unsigned int)iStatus); MsgId = CFE_SB_GetMsgId(MsgPtr); switch (MsgId) { case FAC_CMD_MID: iStatus = ProcessNewAppCmds(MsgPtr); - if (iStatus == FAC_ERR_INVALID_POINTER) + if (iStatus != CFE_SUCCESS) { - FAC_CmdsTaskData.uiCmdsRunStatus = FAC_TASK_ERROR; goto RcvCmdMsg_Exit_Tag; } break; @@ -177,34 +188,33 @@ int32 AppCommandProcess::RcvCmdMsg(int32 iBlocking) */ default: - /* Bump the command error counter for an unknown command. - * (This should only occur if it was subscribed to with this - * pipe, but not handled in this switch-case.) */ - FAC_MainAppData.HkTlm.usCmdErrCnt++; + /* Bump the command error counter for an unknown command. + * (This should only occur if it was subscribed to with this + * pipe, but not handled in this switch-case.) */ + FAC_AppData.HkTlm.usCmdErrCnt++; CFE_EVS_SendEvent(FAC_MSGID_ERR_EID, CFE_EVS_ERROR, "Recvd invalid CMD msgId (0x%04X)", (unsigned short)MsgId); break; } } - else if (iStatus == CFE_SB_NO_MESSAGE) - { - iStatus = CFE_SUCCESS; - } - else - { - CFE_EVS_SendEvent(FAC_PIPE_ERR_EID, CFE_EVS_ERROR, - "CMD pipe read error (0x%08X)", (unsigned int)iStatus); - FAC_CmdsTaskData.uiCmdsRunStatus = FAC_TASK_ERROR; - goto RcvCmdMsg_Exit_Tag; - } + } + else if (iStatus == CFE_SB_NO_MESSAGE) + { + iStatus = CFE_SUCCESS; + } + else + { + CFE_EVS_SendEvent(FAC_PIPE_ERR_EID, CFE_EVS_ERROR, + "CMD pipe read error (0x%08X)", (unsigned int)iStatus); + goto RcvCmdMsg_Exit_Tag; } RcvCmdMsg_Exit_Tag: return iStatus; } -int32 AppCommandProcess::InitCmdsTask() +int32 AppCommandProcess::InitCmdsPipe() { int32 iStatus = CFE_SUCCESS; @@ -217,12 +227,12 @@ int32 AppCommandProcess::InitCmdsTask() CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, "Failed to subscribe to FAC_CMD_MID. (0x%08X)", (unsigned int)iStatus); - goto InitCmdsTask_Exit_Tag; + goto InitCmdsPipe_Exit_Tag; } else { CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, - "InitCmdsTask: Successfully subscribed to FAC_CMD_MID (0x%08X)\n", + "InitCmdsPipe: Successfully subscribed to FAC_CMD_MID (0x%08X)\n", (unsigned int)iStatus); } } @@ -231,91 +241,9 @@ int32 AppCommandProcess::InitCmdsTask() CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, "Failed to create CMD pipe (0x%08X)", (unsigned int)iStatus); - goto InitCmdsTask_Exit_Tag; + goto InitCmdsPipe_Exit_Tag; } -InitCmdsTask_Exit_Tag: +InitCmdsPipe_Exit_Tag: return iStatus; } - -void CmdsTask() -{ - int32 iStatus = CFE_SUCCESS; - CFE_ES_TaskInfo_t CmdsTaskInfo; - - iStatus = CFE_ES_RegisterChildTask(); - if (iStatus != CFE_SUCCESS) - { - CFE_ES_WriteToSysLog("FAC - Failed to register the CmdsTask (0x%08X)\n", - (unsigned int)iStatus); - FAC_CmdsTaskData.uiCmdsRunStatus = FAC_TASK_ERROR; - goto CmdsTask_Exit_Tag; - } - - iStatus = CFE_ES_GetTaskInfo(&CmdsTaskInfo, FAC_CmdsTaskData.CmdsTaskId); //check taskId and appId - if (iStatus != CFE_SUCCESS) - { - CFE_ES_WriteToSysLog("FAC - Failed to Get Cmds task info(0x%08X)\n", - (unsigned int)iStatus); - FAC_CmdsTaskData.uiCmdsRunStatus = FAC_TASK_ERROR; - goto CmdsTask_Exit_Tag; - } - else - { - CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, - "Cmds TaskInfo: TaskID: 0x%08X, TaskName: %s, AppId: 0x%08X, AppName:%s (0x%08X)\n", - (unsigned int)CmdsTaskInfo.TaskId, CmdsTaskInfo.TaskName, - (unsigned int)CmdsTaskInfo.AppId, CmdsTaskInfo.AppName, (unsigned int)iStatus); - } - - CFE_ES_IncrementTaskCounter(); - - FAC_CmdsTaskData.pCmds = new AppCommandProcess(); - if (FAC_CmdsTaskData.pCmds != NULL) - { - iStatus = FAC_CmdsTaskData.pCmds->InitCmdsTask(); - if (iStatus != CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to init cmds task (0x%08X)", - (unsigned int)iStatus); - FAC_CmdsTaskData.uiCmdsRunStatus = FAC_TASK_ERROR; - goto CmdsTask_Exit_Tag; - } - else - { - CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, - "Succeeded to init cmds task: cmds task status: 0x%08X (0x%08X)", - (unsigned int)FAC_CmdsTaskData.uiCmdsRunStatus, - (unsigned int)iStatus); - } - } - else - { - FAC_CmdsTaskData.uiCmdsRunStatus = FAC_TASK_ERROR; - goto CmdsTask_Exit_Tag; - } - - if (FAC_CmdsTaskData.uiCmdsRunStatus == FAC_TASK_RUN) - { - iStatus = FAC_CmdsTaskData.pCmds->RcvCmdMsg(FAC_CMD_PIPE_PEND_TIME); - if (iStatus != CFE_SUCCESS) - { - FAC_CmdsTaskData.uiCmdsRunStatus = FAC_TASK_ERROR; - goto CmdsTask_Exit_Tag; - } - } - -CmdsTask_Exit_Tag: - if (FAC_CmdsTaskData.pCmds != NULL) - { - delete FAC_CmdsTaskData.pCmds; - FAC_CmdsTaskData.pCmds = NULL; - } - CFE_ES_WriteToSysLog("FAC - CmdsTask: Error detected. Task will exit (0x%08X)\n", - (unsigned int)iStatus); - FAC_MainAppData.uiAppRunStatus = CFE_ES_APP_EXIT; - CFE_ES_ExitChildTask(); - - return; -} diff --git a/apps/fac/fsw/src/fac_cmds_task.hpp b/apps/fac/fsw/src/fac_cmds_utils.hpp similarity index 87% rename from apps/fac/fsw/src/fac_cmds_task.hpp rename to apps/fac/fsw/src/fac_cmds_utils.hpp index 31b0bb10b..ca9cf9547 100644 --- a/apps/fac/fsw/src/fac_cmds_task.hpp +++ b/apps/fac/fsw/src/fac_cmds_utils.hpp @@ -31,8 +31,8 @@ * *****************************************************************************/ -#ifndef FAC_CMDS_TASK_HPP -#define FAC_CMDS_TASK_HPP +#ifndef FAC_CMDS_UTILS_HPP +#define FAC_CMDS_UTILS_HPP class AppCommandProcess @@ -41,24 +41,16 @@ class AppCommandProcess AppCommandProcess(); ~AppCommandProcess(); - int32 InitCmdsTask(); + int32 InitCmdsPipe(); int32 RcvCmdMsg(int32 iBlocking); private: CFE_SB_PipeId_t CmdPipeId; int32 ProcessNewAppCmds(CFE_SB_Msg_t *MsgPtr); - boolean VerifyCmdLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen); + int32 VerifyCmdLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen); }; -typedef struct -{ - uint32 CmdsTaskId; - uint32 uiCmdsRunStatus; - AppCommandProcess *pCmds; -} FAC_CmdsTaskData_t; - -void CmdsTask(); #endif diff --git a/apps/fac/fsw/src/fac_config_utils.cpp b/apps/fac/fsw/src/fac_config_utils.cpp deleted file mode 100644 index 088e582bb..000000000 --- a/apps/fac/fsw/src/fac_config_utils.cpp +++ /dev/null @@ -1,202 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name Windhover Labs nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - *****************************************************************************/ - -#include "fac_config_utils.hpp" -#include "fac_cds_task.hpp" - -extern FAC_CdsTaskData_t FAC_CdsTaskData; - - -int32 FAC_InitConfigTbl() -{ - int32 iStatus = CFE_SUCCESS; - - /* Register Config table */ - iStatus = CFE_TBL_Register(&FAC_CdsTaskData.ConfigTblHdl, - FAC_CONFIG_TABLENAME, - (sizeof(FAC_ConfigTblEntry_t) * FAC_CONFIG_TABLE_MAX_ENTRIES), - CFE_TBL_OPT_DEFAULT, - FAC_ValidateConfigTbl); - if (iStatus == CFE_SUCCESS) - { - CFE_EVS_SendEvent(FAC_INIT_INF_EID, CFE_EVS_INFORMATION, - "Succeeded to register Config table (0x%08X)", - (unsigned int)iStatus); - } - else if (iStatus == CFE_TBL_INFO_RECOVERED_TBL) - { - CFE_EVS_SendEvent(FAC_INIT_INF_EID, CFE_EVS_INFORMATION, - "Succeeded to recover Config table (0x%08X)", - (unsigned int)iStatus); - } - else if (iStatus == CFE_TBL_WARN_NOT_CRITICAL) - { - CFE_EVS_SendEvent(FAC_INIT_INF_EID, CFE_EVS_INFORMATION, - "Not Critical Initialization Error of Config table (0x%08X)", - (unsigned int)iStatus); - } - else - { - /* Note, a critical table could return another nominal code. If this table is - * made critical this logic would have to change. */ - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to register Config table (0x%08X)", - (unsigned int)iStatus); - goto FAC_InitConfigTbl_Exit_Tag; - } - - /* Load Config table file */ - iStatus = CFE_TBL_Load(FAC_CdsTaskData.ConfigTblHdl, CFE_TBL_SRC_FILE, - FAC_CONFIG_TABLE_FILENAME); - if (iStatus != CFE_SUCCESS) - { - /* Note, CFE_SUCCESS is for a successful full table load. If a partial table - load is desired then this logic would have to change. */ - CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, - "Failed to load Config Table (0x%08X)", - (unsigned int)iStatus); - goto FAC_InitConfigTbl_Exit_Tag; - } - else - { - CFE_EVS_SendEvent(FAC_INIT_INF_EID, CFE_EVS_INFORMATION, - "Succeeded to load Config Table (0x%08X)", - (unsigned int)iStatus); - } - -FAC_InitConfigTbl_Exit_Tag: - return (iStatus); -} - -int32 FAC_ValidateConfigTbl(void* ConfigTblPtr) -{ - int32 iStatus = CFE_SUCCESS; - FAC_ConfigTblEntry_t* FAC_ConfigTblPtr = (FAC_ConfigTblEntry_t*)(ConfigTblPtr); - - if (ConfigTblPtr == NULL) - { - iStatus = -1; - goto FAC_ValidateConfigTbl_Exit_Tag; - } - - /* TODO: Add code to validate new data values here. - ** - ** Examples: - ** if (FAC_ConfigTblPtr->iParam <= 16) { - ** (void) CFE_EVS_SendEvent(FAC_CONFIG_TABLE_INF_EID, CFE_EVS_ERROR, - * "Invalid value for Config parameter sParam (%d)", - ** FAC_ConfigTblPtr->iParam); - ** } - **/ - -FAC_ValidateConfigTbl_Exit_Tag: - return (iStatus); -} - -void FAC_ProcessNewConfigTbl() -{ - /* TODO: Add code to set new Config parameters with new values here. - ** - ** Examples: - ** - ** FAC_MainAppData.latest_sParam = FAC_MainAppData.ConfigTblPtr->sParam; - ** FAC_MainAppData.latest_fParam = FAC_MainAppData.ConfigTblPtr->fParam; - */ -} - -int32 FAC_AcquireConfigPointers() -{ - int32 iStatus = CFE_SUCCESS; - - /* - ** Release the table - */ - /* TODO: This return value can indicate success, error, or that the info has been - * updated. We ignore this return value in favor of checking CFE_TBL_Manage(), but - * be sure this is the behavior you want. */ - CFE_TBL_ReleaseAddress(FAC_CdsTaskData.ConfigTblHdl); - - /* - ** Manage the table - */ - iStatus = CFE_TBL_Manage(FAC_CdsTaskData.ConfigTblHdl); - if ((iStatus != CFE_SUCCESS) && (iStatus != CFE_TBL_INFO_UPDATED)) - { - CFE_EVS_SendEvent(FAC_CONFIG_TABLE_ERR_EID, CFE_EVS_ERROR, - "Failed to manage Config table (0x%08X)", - (unsigned int)iStatus); - goto FAC_AcquireConfigPointers_Exit_Tag; - } - else - { -#if 0 - CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, - "Succeeded to manage Config Table (0x%08X)", - (unsigned int)iStatus); -#endif - } - - /* - ** Get a pointer to the table - */ - iStatus = CFE_TBL_GetAddress((void**)&FAC_CdsTaskData.ConfigTblPtr, - FAC_CdsTaskData.ConfigTblHdl); - if (iStatus == CFE_SUCCESS) - { -#if 0 - CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, - "Succeeded to get address @@@@@@@@@ (0x%08X)", - (unsigned int)iStatus); -#endif - } - else if (iStatus == CFE_TBL_INFO_UPDATED) - { -#if 0 - CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, - "Succeeded to get Updated add !!!!!!!!!!!!!!! (0x%08X)", - (unsigned int)iStatus); -#endif - FAC_ProcessNewConfigTbl(); - iStatus = CFE_SUCCESS; - } - else - { - FAC_CdsTaskData.ConfigTblPtr = NULL; - CFE_EVS_SendEvent(FAC_CONFIG_TABLE_ERR_EID, CFE_EVS_ERROR, - "Failed to get Config table's address (0x%08X)", - (unsigned int)iStatus); - } - -FAC_AcquireConfigPointers_Exit_Tag: - return (iStatus); -} diff --git a/apps/fac/fsw/src/fac_data_utils.cpp b/apps/fac/fsw/src/fac_data_utils.cpp new file mode 100644 index 000000000..d2070dc8a --- /dev/null +++ b/apps/fac/fsw/src/fac_data_utils.cpp @@ -0,0 +1,539 @@ +/**************************************************************************** + * + * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + +#include +#include + +#include + +#include "fac_tbldefs.h" +#include "fac_app.hpp" +#include "fac_data_utils.hpp" +#include "fac_msgids.h" + +#include + + +extern FAC_AppData_t FAC_AppData; + + +AppDataProcess::AppDataProcess() +{ +} + +AppDataProcess::~AppDataProcess() +{ +} + +void AppDataProcess::SendOutData() +{ +#if 0 // check this + /* TODO: Add code to update output data, if needed, here. */ + + CFE_SB_TimeStampMsg((CFE_SB_Msg_t*)&OutData); + int32 iStatus = CFE_SB_SendMsg((CFE_SB_Msg_t*)&OutData); + if (iStatus != CFE_SUCCESS) + { + /* TODO: Decide what to do if the send message fails. */ + } +#endif +} + +void AppDataProcess::ProcessNewConfigTbl() +{ + /* TODO: Add code to set new Config parameters with new values here. + ** + ** Examples: + ** + ** FAC_AppData.latest_sParam = FAC_AppData.ConfigTblPtr->sParam; + ** FAC_AppData.latest_fParam = FAC_AppData.ConfigTblPtr->fParam; + */ +} + +int32 AppDataProcess::AcquireConfigPointers() +{ + int32 iStatus = CFE_SUCCESS; + + /* + ** Release the table + */ + /* TODO: This return value can indicate success, error, or that the info has been + * updated. We ignore this return value in favor of checking CFE_TBL_Manage(), but + * be sure this is the behavior you want. */ + CFE_TBL_ReleaseAddress(FAC_AppData.ConfigTblHdl); + + /* + ** Manage the table + */ + iStatus = CFE_TBL_Manage(FAC_AppData.ConfigTblHdl); + if ((iStatus != CFE_SUCCESS) && (iStatus != CFE_TBL_INFO_UPDATED)) + { + CFE_EVS_SendEvent(FAC_CONFIG_TABLE_ERR_EID, CFE_EVS_ERROR, + "Failed to manage Config table (0x%08X)", + (unsigned int)iStatus); + goto AcquireConfigPointers_Exit_Tag; + } + else + { +#if 0 + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "Succeeded to manage Config Table (0x%08X)", + (unsigned int)iStatus); +#endif + } + + /* + ** Get a pointer to the table + */ + iStatus = CFE_TBL_GetAddress((void**)&FAC_AppData.ConfigTblPtr, FAC_AppData.ConfigTblHdl); + if (iStatus == CFE_SUCCESS) + { +#if 0 + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "Succeeded to get table address @@@@@@@@@ (0x%08X)", + (unsigned int)iStatus); +#endif + } + else if (iStatus == CFE_TBL_INFO_UPDATED) + { +#if 0 + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "Succeeded to get updated table address !!!!!!!!!!!! (0x%08X)", + (unsigned int)iStatus); +#endif + ProcessNewConfigTbl(); + iStatus = CFE_SUCCESS; + } + else + { + FAC_AppData.ConfigTblPtr = NULL; + CFE_EVS_SendEvent(FAC_CONFIG_TABLE_ERR_EID, CFE_EVS_ERROR, + "Failed to get Config table address (0x%08X)", + (unsigned int)iStatus); + FAC_AppData.uiAppRunStatus = CFE_ES_APP_ERROR; + goto AcquireConfigPointers_Exit_Tag; + } + +AcquireConfigPointers_Exit_Tag: + return (iStatus); +} + +int32 AppDataProcess::ValidateConfigTbl(void* ConfigTblPtr) +{ + int32 iStatus = CFE_SUCCESS; + FAC_ConfigTbl_t* CfgTblPtr = (FAC_ConfigTbl_t*)(ConfigTblPtr); + + if (ConfigTblPtr == NULL) + { + iStatus = FAC_ERR_INVALID_POINTER; + goto ValidateConfigTbl_Exit_Tag; + } + + /* TODO: Add code to validate new data values here. + ** + ** Examples: + ** if (CfgTblPtr->iParam <= 16) { + ** (void) CFE_EVS_SendEvent(FAC_CONFIG_TABLE_INF_EID, CFE_EVS_ERROR, + * "Invalid value for Config parameter sParam (%d)", + ** CfgTblPtr->iParam); + ** } + **/ + +ValidateConfigTbl_Exit_Tag: + return (iStatus); +} + +int32 AppDataProcess::VerifyDataMsgLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen) +{ + uint16 usMsgLen = 0; + int32 iStatus = CFE_SUCCESS; + + if (MsgPtr != NULL) + { + usMsgLen = CFE_SB_GetTotalMsgLength(MsgPtr); + + if (usExpectedLen != usMsgLen) + { + iStatus = FAC_ERR_MSG_LENGTH; + CFE_SB_MsgId_t MsgId = CFE_SB_GetMsgId(MsgPtr); + uint16 usCmdCode = CFE_SB_GetCmdCode(MsgPtr); + + CFE_EVS_SendEvent(FAC_MSGLEN_ERR_EID, CFE_EVS_ERROR, + "Rcvd invalid msgLen: msgId=0x%08X, cmdCode=%d, " + "msgLen=%d, expectedLen=%d", MsgId, usCmdCode, + usMsgLen, usExpectedLen); + } + // Add more to check + } + else + { + iStatus = FAC_ERR_INVALID_POINTER; + + } + + return (iStatus); +} + +void AppDataProcess::HandleAirspeed() +{ + return; +} + +void AppDataProcess::HandleVehAttitude() +{ + return; +} + +void AppDataProcess::HandleVehAttitudeSetPoint() +{ + return; +} + +void AppDataProcess::HandleVehControlMode() +{ + return; +} + +void AppDataProcess::HandleParameterUpdate() +{ + return; +} + +void AppDataProcess::HandleManualControlSetpoint() +{ + return; +} + +void AppDataProcess::HandleVehGlobalPosition() +{ + return; +} + +void AppDataProcess::HandleVehStatus() +{ + return; +} + +void AppDataProcess::HandleVehLandDetected() +{ + return; +} + +void AppDataProcess::HandleBatteryStatus() +{ + return; +} + +int32 AppDataProcess::RcvDataMsg(int32 iBlocking) +{ + int32 iStatus = CFE_SUCCESS; + CFE_SB_Msg_t *DataMsgPtr = NULL; + CFE_SB_MsgId_t DataMsgId; + + iStatus = CFE_SB_RcvMsg(&DataMsgPtr, DataPipeId, iBlocking); + if (iStatus == CFE_SUCCESS) + { + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "RcvDataMsg:Receivd message (0x%08X)", + (unsigned int)iStatus); + if (FAC_AppData.bAppAwaken == TRUE) + { + DataMsgId = CFE_SB_GetMsgId(DataMsgPtr); + switch (DataMsgId) + { + case PX4_AIRSPEED_MID: + HandleAirspeed(); + break; + + case PX4_VEHICLE_ATTITUDE_MID: + HandleVehAttitude(); + break; + + case PX4_VEHICLE_ATTITUDE_SETPOINT_MID: + HandleVehAttitudeSetPoint(); + break; + + case PX4_VEHICLE_CONTROL_MODE_MID: + HandleVehControlMode(); + break; + + case PX4_PARAMETER_UPDATE_MID: + HandleParameterUpdate(); + break; + + case PX4_MANUAL_CONTROL_SETPOINT_MID: + HandleManualControlSetpoint(); + break; + + case PX4_VEHICLE_GLOBAL_POSITION_MID: + HandleVehGlobalPosition(); + break; + + case PX4_VEHICLE_STATUS_MID: + HandleVehStatus(); + break; + + case PX4_VEHICLE_LAND_DETECTED_MID: + HandleVehLandDetected(); + break; + + case PX4_BATTERY_STATUS_MID: + HandleBatteryStatus(); + break; + + default: + CFE_EVS_SendEvent(FAC_MSGID_ERR_EID, CFE_EVS_ERROR, + "Recvd invalid Data msgId (0x%04X)", + (unsigned short)DataMsgId); + goto RcvDataMsg_Exit_Tag; + } + } + } + else if (iStatus == CFE_SB_NO_MESSAGE) + { + iStatus = CFE_SUCCESS; + } + else + { + CFE_EVS_SendEvent(FAC_PIPE_ERR_EID, CFE_EVS_ERROR, + "Data pipe read error (0x%08X)", (unsigned int)iStatus); + goto RcvDataMsg_Exit_Tag; + } + + iStatus = AcquireConfigPointers(); // Decide where to execute this + if (iStatus != CFE_SUCCESS) + { + CFE_EVS_SendEvent(FAC_CONFIG_TABLE_ERR_EID, CFE_EVS_ERROR, + "AcquireConfigPointers error (0x%08X)", + (unsigned int)iStatus); + goto RcvDataMsg_Exit_Tag; + } + else + { +#if 0 + CFE_EVS_SendEvent(FAC_CONFIG_TABLE_INF_EID, CFE_EVS_INFORMATION, + "Succeeded to AcquireConfigPointers ********** (0x%08X)", + (unsigned int)iStatus); +#endif + } + +RcvDataMsg_Exit_Tag: + return iStatus; +} + +int32 AppDataProcess::Subscribe(CFE_SB_MsgId_t MsgId) +{ + int32 iStatus = CFE_SUCCESS; + + iStatus = CFE_SB_Subscribe(MsgId, DataPipeId); + if (iStatus != CFE_SUCCESS) + { + CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, + "Failed to subscribe to MID:0x%04X (0x%08X)", + (unsigned short)MsgId, (unsigned int)iStatus); + } + + return (iStatus); +} + +int32 AppDataProcess::InitConfigTbl() +{ + int32 iStatus = CFE_SUCCESS; + + /* Register Config table */ + iStatus = CFE_TBL_Register(&FAC_AppData.ConfigTblHdl, FAC_CONFIG_TABLENAME, + sizeof(FAC_ConfigTbl_t), CFE_TBL_OPT_DEFAULT, + ValidateConfigTbl); + if (iStatus == CFE_SUCCESS) + { + CFE_EVS_SendEvent(FAC_INIT_INF_EID, CFE_EVS_INFORMATION, + "Succeeded to register Config table (0x%08X)", + (unsigned int)iStatus); + } + else if (iStatus == CFE_TBL_INFO_RECOVERED_TBL) + { + CFE_EVS_SendEvent(FAC_INIT_INF_EID, CFE_EVS_INFORMATION, + "Succeeded to recover Config table (0x%08X)", + (unsigned int)iStatus); + } + else if (iStatus == CFE_TBL_WARN_NOT_CRITICAL) + { + CFE_EVS_SendEvent(FAC_INIT_INF_EID, CFE_EVS_INFORMATION, + "Not Critical Initialization Error of Config table (0x%08X)", + (unsigned int)iStatus); + } + else + { + /* Note, a critical table could return another nominal code. If this table is + * made critical this logic would have to change. */ + CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, + "Failed to register Config table (0x%08X)", + (unsigned int)iStatus); + goto InitConfigTbl_Exit_Tag; + } + + /* Load Config table file */ + iStatus = CFE_TBL_Load(FAC_AppData.ConfigTblHdl, CFE_TBL_SRC_FILE, FAC_CONFIG_TABLE_FILENAME); + if (iStatus != CFE_SUCCESS) + { + /* Note, CFE_SUCCESS is for a successful full table load. If a partial table + load is desired then this logic would have to change. */ + CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, + "Failed to load Config Table (0x%08X)", + (unsigned int)iStatus); + goto InitConfigTbl_Exit_Tag; + } + else + { + CFE_EVS_SendEvent(FAC_INIT_INF_EID, CFE_EVS_INFORMATION, + "Succeeded to load Config Table (0x%08X)", + (unsigned int)iStatus); + } + +InitConfigTbl_Exit_Tag: + return (iStatus); +} + +int32 AppDataProcess::InitTables() +{ + int32 iStatus = CFE_SUCCESS; + + iStatus = InitData(); + if (iStatus != CFE_SUCCESS) + { + CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, + "Failed to initialize Data (0x%08X)", + (unsigned int)iStatus); + goto InitTables_Exit_Tag; + } + + iStatus = InitConfigTbl(); + if (iStatus != CFE_SUCCESS) + { + CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, + "Failed to init config tables (0x%08X)", + (unsigned int)iStatus); + goto InitTables_Exit_Tag; + } + +InitTables_Exit_Tag: + return iStatus; +} + +int32 AppDataProcess::InitData() +{ + int32 iStatus = CFE_SUCCESS; + + /* Init input data */ + memset((void*)&InData, 0x00, sizeof(InData)); + + /* Init output data */ +// memset((void*)&OutData, 0x00, sizeof(OutData)); // Check for memset + /* memset entire OutData length and set the stream ID and length and + * restore sequence count if not TRUE + */ +// check cfe function for length + CFE_SB_InitMsg(&OutData, FAC_OUT_DATA_MID, sizeof(OutData), TRUE); // check length + + return (iStatus); +} + +int32 AppDataProcess::InitDataPipe() +{ + int32 iStatus = CFE_SUCCESS; + + iStatus = CFE_SB_CreatePipe(&DataPipeId, FAC_DATA_PIPE_DEPTH, FAC_DATA_PIPE_NAME); + if (iStatus == CFE_SUCCESS) + { + iStatus = Subscribe(PX4_AIRSPEED_MID); + if (iStatus != CFE_SUCCESS) + { + goto InitDataPipe_Exit_Tag; + } + iStatus = Subscribe(PX4_VEHICLE_ATTITUDE_MID); + if (iStatus != CFE_SUCCESS) + { + goto InitDataPipe_Exit_Tag; + } + iStatus = Subscribe(PX4_VEHICLE_ATTITUDE_SETPOINT_MID); + if (iStatus != CFE_SUCCESS) + { + goto InitDataPipe_Exit_Tag; + } + iStatus = Subscribe(PX4_VEHICLE_CONTROL_MODE_MID); + if (iStatus != CFE_SUCCESS) + { + goto InitDataPipe_Exit_Tag; + } + iStatus = Subscribe(PX4_PARAMETER_UPDATE_MID); + if (iStatus != CFE_SUCCESS) + { + goto InitDataPipe_Exit_Tag; + } + iStatus = Subscribe(PX4_MANUAL_CONTROL_SETPOINT_MID); + if (iStatus != CFE_SUCCESS) + { + goto InitDataPipe_Exit_Tag; + } + iStatus = Subscribe(PX4_VEHICLE_GLOBAL_POSITION_MID); + if (iStatus != CFE_SUCCESS) + { + goto InitDataPipe_Exit_Tag; + } + iStatus = Subscribe(PX4_VEHICLE_STATUS_MID); + if (iStatus != CFE_SUCCESS) + { + goto InitDataPipe_Exit_Tag; + } + iStatus = Subscribe(PX4_VEHICLE_LAND_DETECTED_MID); + if (iStatus != CFE_SUCCESS) + { + goto InitDataPipe_Exit_Tag; + } + iStatus = Subscribe(PX4_BATTERY_STATUS_MID); + if (iStatus != CFE_SUCCESS) + { + goto InitDataPipe_Exit_Tag; + } + } + else + { + CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, + "Failed to create Data pipe (0x%08X)", + (unsigned int)iStatus); + goto InitDataPipe_Exit_Tag; + } + +InitDataPipe_Exit_Tag: + return iStatus; +} diff --git a/apps/fac/fsw/src/fac_cds_task.hpp b/apps/fac/fsw/src/fac_data_utils.hpp similarity index 67% rename from apps/fac/fsw/src/fac_cds_task.hpp rename to apps/fac/fsw/src/fac_data_utils.hpp index dc304b12e..ba906d9fc 100644 --- a/apps/fac/fsw/src/fac_cds_task.hpp +++ b/apps/fac/fsw/src/fac_data_utils.hpp @@ -31,54 +31,32 @@ * *****************************************************************************/ -#ifndef FAC_CDS_TASK_HPP -#define FAC_CDS_TASK_HPP +#ifndef FAC_DATA_UTILS_HPP +#define FAC_DATA_UTILS_HPP -#include "fac_app.hpp" -#include "fac_tbldefs.h" -#include "fac_msg.h" - -enum -{ - FAC_GROUP_RESERVED = 0, - FAC_GROUP_NONE, - FAC_GROUP_Actuator, - FAC_GROUP_Fw, - FAC_GROUP_Vehicle, - FAC_GROUP_Sensor -}; +#if 0 typedef enum { FAC_PX4_INC_ERR_CTR = -1, /* Error detected, increment counter */ FAC_PX4_DONT_INC_CTR = 0, /* No errors detected but don't increment counter */ FAC_PX4_INC_TLM_CTR = 1 /* No errors detected and increment counter */ } FAC_PX4MsgFuncRet_t; - -typedef FAC_PX4MsgFuncRet_t (*FAC_PX4MsgFuncPtr_t)(const uint8 *Payload); - -typedef struct -{ - uint16 cdsGrpId; - uint32 ExpectedLength; - FAC_PX4MsgFuncPtr_t MsgFuncPtr; -} FAC_CDS_PX4HandlerTblRec_t; - -typedef struct -{ - uint16 msgId; - uint16 cdsGrpId; -} FAC_CDS_DataGrpTbl_t; +#endif -class CriticalDataStorage +class AppDataProcess { public: - CriticalDataStorage(); - ~CriticalDataStorage(); + AppDataProcess(); + ~AppDataProcess(); - int32 InitCdsTask(); + int32 InitDataPipe(); + int32 InitTables(); int32 RcvDataMsg(int32 iBlocking); + void SendOutData(); + + static int32 ValidateConfigTbl(void* ConfigTblPtr); private: CFE_SB_PipeId_t DataPipeId; @@ -86,29 +64,27 @@ class CriticalDataStorage FAC_InData_t InData; FAC_OutData_t OutData; - int32 InitPipe(); int32 InitData(); - int32 GetCdsGrpId(CFE_SB_MsgId_t DataMsgId); - - boolean VerifyCdsMsgLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen); -}; + int32 InitConfigTbl(); + int32 Subscribe(CFE_SB_MsgId_t MsgId); + int32 VerifyDataMsgLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen); -typedef struct -{ - uint32 CdsTaskId; - uint32 uiCdsRunStatus; - - CriticalDataStorage *pCds; + void HandleAirspeed(); + void HandleVehAttitude(); + void HandleVehAttitudeSetPoint(); + void HandleVehControlMode(); + void HandleParameterUpdate(); + void HandleManualControlSetpoint(); + void HandleVehGlobalPosition(); + void HandleVehStatus(); + void HandleVehLandDetected(); + void HandleBatteryStatus(); - CFE_TBL_Handle_t ConfigTblHdl; - CFE_ES_CDSHandle_t CdsTblHdl; + int32 AcquireConfigPointers(); + void ProcessNewConfigTbl(); - FAC_ConfigTblEntry_t *ConfigTblPtr; - FAC_CdsTbl_t CdsTbl; - -} FAC_CdsTaskData_t; +}; -void CdsTask(); #endif diff --git a/apps/fac/fsw/src/fac_events.h b/apps/fac/fsw/src/fac_events.h index bda41b290..cf6e4214a 100644 --- a/apps/fac/fsw/src/fac_events.h +++ b/apps/fac/fsw/src/fac_events.h @@ -63,7 +63,6 @@ typedef enum { FAC_INF_EID, /** \brief 'FAC Initialized. Version \%d.\%d.\%d' -** \event 'FAC Initialized. Version \%d.\%d.\%d' ** ** \par Type: INFORMATION ** @@ -79,7 +78,6 @@ typedef enum { FAC_INIT_INF_EID, /** \brief 'FAC - ' -** \event 'FAC - ' ** ** \par Type: INFORMATION ** @@ -91,7 +89,6 @@ typedef enum { FAC_CONFIG_TABLE_INF_EID, /** \brief 'FAC - ' -** \event 'FAC - ' ** ** \par Type: INFORMATION ** @@ -103,7 +100,6 @@ typedef enum { FAC_CDS_INF_EID, /** \brief 'FAC - Recvd $x cmd (%us)' -** \event 'FAC - Recvd $x cmd (%us)' ** ** \par Type: INFORMATION ** @@ -116,7 +112,6 @@ typedef enum { FAC_CMD_INF_EID, /** \brief 'FAC - ' -** \event 'FAC - ' ** ** \par Type: ERROR ** @@ -128,7 +123,6 @@ typedef enum { FAC_ERR_EID, /** \brief 'FAC - ' -** \event 'FAC - ' ** ** \par Type: ERROR ** @@ -141,7 +135,6 @@ typedef enum { FAC_INIT_ERR_EID, /** \brief 'FAC - ' -** \event 'FAC - ' ** ** \par Type: ERROR ** @@ -154,7 +147,6 @@ typedef enum { FAC_CONFIG_TABLE_ERR_EID, /** \brief 'FAC - ' -** \event 'FAC - ' ** ** \par Type: ERROR ** @@ -167,7 +159,6 @@ typedef enum { FAC_CDS_ERR_EID, /** \brief 'FAC - $commandError' -** \event 'FAC - $commandError' ** ** \par Type: ERROR ** @@ -180,7 +171,6 @@ typedef enum { FAC_CMD_ERR_EID, /** \brief 'FAC: SB pipe read error (0x%08X), app will exit' -** \event 'FAC: SB pipe read error (0x%08X), app will exit' ** ** \par Type: ERROR ** @@ -193,7 +183,6 @@ typedef enum { FAC_PIPE_ERR_EID, /** \brief 'FAC - Recvd invalid $type msgId (0x%04x)' -** \event 'FAC - Recvd invalid $type msgId (0x%04x)' ** ** \par Type: ERROR ** @@ -206,7 +195,6 @@ typedef enum { FAC_MSGID_ERR_EID, /** \brief 'FAC - Rcvd invalid msgLen: msgId=0x%08X, cmdCode=%d, msgLen=%d, expectedLen=%d" -** \event 'FAC - Rcvd invalid msgLen: msgId=0x%08X, cmdCode=%d, msgLen=%d, expectedLen=%d" ** ** \par Type: ERROR ** @@ -224,9 +212,4 @@ typedef enum { } FAC_EventIds_t; - #endif /* FAC_EVENTS_H */ - -/************************/ -/* End of File Comment */ -/************************/ diff --git a/apps/fac/fsw/src/fac_fac.hpp b/apps/fac/fsw/src/fac_fac.hpp index 57246ad09..bb328247f 100644 --- a/apps/fac/fsw/src/fac_fac.hpp +++ b/apps/fac/fsw/src/fac_fac.hpp @@ -34,19 +34,6 @@ #ifndef FAC_FPC_HPP #define FAC_FPC_HPP -#include "cfe.h" - -#include "fac_platform_cfg.h" -#include "fac_mission_cfg.h" -#include "fac_private_ids.h" -#include "fac_private_types.h" -#include "fac_perfids.h" -#include "fac_msgids.h" -#include "fac_msg.h" -#include "fac_events.h" -#include "fac_config_utils.hpp" -#include "fac_cds_utils.hpp" - class FixedwingAttitudeControl { @@ -54,7 +41,9 @@ class FixedwingAttitudeControl FixedwingAttitudeControl(); ~FixedwingAttitudeControl(); - int32 InitApp(); + int32 InitEvent(); + int32 InitSchPipe(); + int32 InitHk(); int32 RcvSchMsg(int32 iBlocking); private: @@ -62,13 +51,10 @@ class FixedwingAttitudeControl CFE_EVS_BinFilter_t EventTbl[FAC_EVT_CNT]; - int32 InitEvent(); - int32 InitPipe(); - int32 InitHk(); - - int32 WakeupMsg(CFE_SB_Msg_t *MsgPtr, CFE_SB_MsgId_t MsgId); + int32 Execute(); + int32 WakeupValidate(CFE_SB_Msg_t *MsgPtr, CFE_SB_MsgId_t MsgId); int32 SendHkMsg(CFE_SB_Msg_t *MsgPtr, CFE_SB_MsgId_t MsgId); - boolean VerifySchCmdLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen); + int32 VerifySchCmdLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen); // void IncrementCmdCount(); // void IncrementCmdErr(); void ReportHousekeeping(); diff --git a/apps/fac/fsw/src/fac_msg.h b/apps/fac/fsw/src/fac_msg.h index 1800d661f..3b8b888cf 100644 --- a/apps/fac/fsw/src/fac_msg.h +++ b/apps/fac/fsw/src/fac_msg.h @@ -34,24 +34,11 @@ #ifndef FAC_MSG_H #define FAC_MSG_H -/************************************************************************ -** Pragmas -*************************************************************************/ - -/************************************************************************ -** Includes -*************************************************************************/ #include "cfe.h" - -/************************************************************************ -** Local Defines -*************************************************************************/ - /************************************************************************ ** FAC Command Codes *************************************************************************/ - /** \faccmd Noop ** ** \par Description @@ -117,10 +104,6 @@ */ #define FAC_RESET_CC (1) -/************************************************************************ -** Local Structure Declarations -*************************************************************************/ - /** ** \brief No Arguments Command ** For command details see #FAC_NOOP_CC, #FAC_RESET_CC @@ -178,7 +161,3 @@ typedef struct #endif /* FAC_MSG_H */ - -/************************/ -/* End of File Comment */ -/************************/ diff --git a/apps/fac/fsw/src/fac_private_ids.h b/apps/fac/fsw/src/fac_private_ids.h index 1181e12d7..97a6fd7f4 100644 --- a/apps/fac/fsw/src/fac_private_ids.h +++ b/apps/fac/fsw/src/fac_private_ids.h @@ -34,44 +34,9 @@ #ifndef FAC_PRIVATE_IDS_H #define FAC_PRIVATE_IDS_H -/************************************************************************ -** Pragmas -*************************************************************************/ - -/************************************************************************ -** Includes -*************************************************************************/ #include "cfe.h" -/************************************************************************ -** Local Defines -*************************************************************************/ - -/************************************************************************ -** Local Structure Declarations -*************************************************************************/ - -/************************************************************************ -** External Global Variables -*************************************************************************/ - -/************************************************************************ -** Global Variables -*************************************************************************/ - -/************************************************************************ -** Local Variables -*************************************************************************/ - -/************************************************************************ -** Local Function Prototypes -*************************************************************************/ - #endif /* FAC_PRIVATE_IDS_H */ - -/************************/ -/* End of File Comment */ -/************************/ diff --git a/apps/fac/fsw/src/fac_private_types.h b/apps/fac/fsw/src/fac_private_types.h index bde35522b..702ad6191 100644 --- a/apps/fac/fsw/src/fac_private_types.h +++ b/apps/fac/fsw/src/fac_private_types.h @@ -37,11 +37,9 @@ #include "cfe.h" -#define FAC_TASK_RUN (0) -#define FAC_TASK_ERROR (1) -#define FAC_TASK_EXIT (2) - -#define FAC_ERR_INVALID_POINTER (-1) +#define FAC_SUCCESS (CFE_SUCCESS) +#define FAC_ERR_MSG_LENGTH (-1) +#define FAC_ERR_INVALID_POINTER (-2) #endif /* FAC_PRIVATE_TYPES_H */ diff --git a/apps/fac/fsw/src/fac_symbols.cpp b/apps/fac/fsw/src/fac_symbols.cpp index b10c15da7..1ad18c0b1 100644 --- a/apps/fac/fsw/src/fac_symbols.cpp +++ b/apps/fac/fsw/src/fac_symbols.cpp @@ -1,3 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + #include "fac_msg.h" FAC_NoArgCmd_t FAC_Noop; diff --git a/apps/fac/fsw/src/fac_tbldefs.h b/apps/fac/fsw/src/fac_tbldefs.h index 89fae2f43..e5929456a 100644 --- a/apps/fac/fsw/src/fac_tbldefs.h +++ b/apps/fac/fsw/src/fac_tbldefs.h @@ -34,98 +34,617 @@ #ifndef FAC_TBLDEFS_H #define FAC_TBLDEFS_H -/************************************************************************ -** Pragmas -*************************************************************************/ - -/************************************************************************ -** Includes -*************************************************************************/ #include "cfe.h" -/************************************************************************ -** Local Defines -*************************************************************************/ -/** - * \brief Defines the number of entries in the table - */ -#define FAC_CONFIG_TABLE_MAX_ENTRIES (1) - -/** - * \brief Defines the table identification name used for table registration. - */ #define FAC_CONFIG_TABLENAME ("CONFIG_TBL") -/** - * \brief Defines the table file name used for table registration. - */ -#define FAC_CDS_TABLENAME ("fac_CdsTbl") - -/************************************************************************ -** Local Structure Declarations -*************************************************************************/ -/** \brief Definition for a single config table entry */ typedef struct { - int32 iParam; - - /* TODO: Add type declaration for config parameters here. - ** - ** Examples: - ** int8/char cParam; - ** int8/char cParams[16]; - ** uint8/unsigned char ucParam; - ** uint8/unsigned char ucParams[16]; - ** - ** int16 sParam; - ** int16 sParams[8]; - ** uint16 usParam; - ** uint16 usParams[8]; - ** - ** int32 iParam; - ** int32 iParams[5]; - ** uint32 uiParam; - ** uint32 uiParams[5]; - ** - ** float fParam; - ** float fParams[3]; - ** - ** double dParam; - ** double dParams[3]; - */ -} FAC_ConfigTblEntry_t; - - -/** \brief Definition for Critical Data Storage (CDS) table entry */ -typedef struct -{ - int32 iParam; + /** + * Attitude Roll Time Constant + * + * This defines the latency between a roll step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit s + * @min 0.4 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + float FW_R_TC; - /* TODO: Add type declaration for CDS data here. */ -} FAC_CdsTbl_t; + /** + * Attitude pitch time constant + * + * This defines the latency between a pitch step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit s + * @min 0.2 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + float FW_P_TC; -/************************************************************************ -** External Global Variables -*************************************************************************/ + /** + * Pitch rate proportional gain. + * + * This defines how much the elevator input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + float FW_PR_P; -/************************************************************************ -** Global Variables -*************************************************************************/ + /** + * Pitch rate integrator gain. + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.5 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + float FW_PR_I; -/************************************************************************ -** Local Variables -*************************************************************************/ + /** + * Maximum positive / up pitch rate. + * + * This limits the maximum pitch up angular rate the controller will output (in + * degrees per second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_P_RMAX_POS; -/************************************************************************ -** Local Function Prototypes -*************************************************************************/ + /** + * Maximum negative / down pitch rate. + * + * This limits the maximum pitch down up angular rate the controller will + * output (in degrees per second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_P_RMAX_NEG; + /** + * Pitch rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + float FW_PR_IMAX; + /** + * Roll rate proportional Gain + * + * This defines how much the aileron input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + float FW_RR_P; -#endif /* FAC_TBLDEFS_H */ + /** + * Roll rate integrator Gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.2 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + float FW_RR_I; + + /** + * Roll integrator anti-windup + * + * The portion of the integrator part in the control surface deflection is limited to this value. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + float FW_RR_IMAX; + + /** + * Maximum roll rate + * + * This limits the maximum roll rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_R_RMAX; + + /** + * Yaw rate proportional gain + * + * This defines how much the rudder input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + float FW_YR_P; + + /** + * Yaw rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.0 + * @max 50.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_YR_I; + + /** + * Yaw rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + float FW_YR_IMAX; + + /** + * Maximum yaw rate + * + * This limits the maximum yaw rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_Y_RMAX; + + /** + * Roll control to yaw control feedforward gain. + * + * This gain can be used to counteract the "adverse yaw" effect for fixed wings. + * When the plane enters a roll it will tend to yaw the nose out of the turn. + * This gain enables the use of a yaw actuator (rudder, airbrakes, ...) to counteract + * this effect. + * + * @min 0.0 + * @decimal 1 + * @increment 0.01 + * @group FW Attitude Control + */ + float FW_RLL_TO_YAW_FF; + + /** + * Enable wheel steering controller + * + * @boolean + * @group FW Attitude Control + */ + float FW_W_EN; + + /** + * Wheel steering rate proportional gain + * + * This defines how much the wheel steering input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + float FW_WR_P; + + /** + * Wheel steering rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.5 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + float FW_WR_I; + + /** + * Wheel steering rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + float FW_WR_IMAX; + + /** + * Maximum wheel steering rate + * + * This limits the maximum wheel steering rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_W_RMAX; + + /** + * Roll rate feed forward + * + * Direct feed forward from rate setpoint to control surface output. Use this + * to obtain a tigher response of the controller without introducing + * noise amplification. + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + float FW_RR_FF; + + /** + * Pitch rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + float FW_PR_FF; + + /** + * Yaw rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + float FW_YR_FF; + + /** + * Wheel steering rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + float FW_WR_FF; + + /** + * Minimal speed for yaw coordination + * + * For airspeeds above this value, the yaw rate is calculated for a coordinated + * turn. Set to a very high value to disable. + * + * @unit m/s + * @min 0.0 + * @max 1000.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_YCO_VMIN; + + /** + * Method used for yaw coordination + * + * The param value sets the method used to calculate the yaw rate + * 0: open-loop zero lateral acceleration based on kinematic constraints + * 1: closed-loop: try to reduce lateral acceleration to 0 by measuring the acceleration + * + * @min 0 + * @max 1 + * @value 0 open-loop + * @value 1 closed-loop + * @group FW Attitude Control + */ + int32 FW_YCO_METHOD; + + /** + * Roll setpoint offset + * + * An airframe specific offset of the roll setpoint in degrees, the value is + * added to the roll setpoint and should correspond to the typical cruise speed + * of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_RSP_OFF; + + /** + * Pitch setpoint offset + * + * An airframe specific offset of the pitch setpoint in degrees, the value is + * added to the pitch setpoint and should correspond to the typical cruise + * speed of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_PSP_OFF; + + /** + * Max manual roll + * + * Max roll for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_MAN_R_MAX; + + /** + * Max manual pitch + * + * Max pitch for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + float FW_MAN_P_MAX; + + /** + * Scale factor for flaps + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + float FW_FLAPS_SCL; + + /** + * Scale factor for flaperons + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + float FW_FLAPERON_SCL; -/************************/ -/* End of File Comment */ -/************************/ + /** + * Disable airspeed sensor + * + * For small wings or VTOL without airspeed sensor this parameter can be used to + * enable flying without an airspeed reading + * + * @boolean + * @group FW Attitude Control + */ + int32 FW_ARSP_MODE; + + /** + * Manual roll scale + * + * Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + float FW_MAN_R_SC; + + /** + * Manual pitch scale + * + * Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + float FW_MAN_P_SC; + + /** + * Manual yaw scale + * + * Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + float FW_MAN_Y_SC; + + /** + * Whether to scale throttle by battery power level + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The fixed wing + * should constantly behave as if it was fully charged with reduced max thrust + * at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group FW Attitude Control + */ + int32 FW_BAT_SCALE_EN; + + /** + * Acro body x max rate. + * + * This is the rate the controller is trying to achieve if the user applies full roll + * stick input in acro mode. + * + * @min 45 + * @max 720 + * @unit degrees + * @group FW Attitude Control + */ + float FW_ACRO_X_MAX; + + /** + * Acro body y max rate. + * + * This is the body y rate the controller is trying to achieve if the user applies full pitch + * stick input in acro mode. + * + * @min 45 + * @max 720 + * @unit degrees + * @group FW Attitude Control + */ + float FW_ACRO_Y_MAX; + + /** + * Acro body z max rate. + * + * This is the body z rate the controller is trying to achieve if the user applies full yaw + * stick input in acro mode. + * + * @min 10 + * @max 180 + * @unit degrees + * @group FW Attitude Control + */ + float FW_ACRO_Z_MAX; + + /** + * Threshold for Rattitude mode + * + * Manual input needed in order to override attitude control rate setpoints + * and instead pass manual stick inputs as rate setpoints + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + float FW_RATT_TH; + +} FAC_ConfigTbl_t; + + +#endif /* FAC_TBLDEFS_H */ diff --git a/apps/fac/fsw/src/fac_version.h b/apps/fac/fsw/src/fac_version.h index c4c032e00..a27b812f0 100644 --- a/apps/fac/fsw/src/fac_version.h +++ b/apps/fac/fsw/src/fac_version.h @@ -78,9 +78,4 @@ #define FAC_REVISION (0) - #endif /* FAC_VERSION_H */ - -/************************/ -/* End of File Comment */ -/************************/ diff --git a/apps/fac/fsw/tables/fac_config.c b/apps/fac/fsw/tables/fac_config.c index 537a980f0..dd1bbc1b0 100644 --- a/apps/fac/fsw/tables/fac_config.c +++ b/apps/fac/fsw/tables/fac_config.c @@ -1,22 +1,6 @@ - -/************************************************************************ -** Pragmas -*************************************************************************/ - -/************************************************************************ -** Includes -*************************************************************************/ #include "cfe_tbl_filedef.h" #include "fac_tbldefs.h" -/************************************************************************ -** Local Defines -*************************************************************************/ - -/************************************************************************ -** Local Structure Definitions -*************************************************************************/ - /** ** \brief The cFE FAC config table definition. ** @@ -33,65 +17,611 @@ static CFE_TBL_FileDef_t CFE_TBL_FileDef = { "FAC_ConfigTbl", "FAC.CONFIG_TBL", "FAC default config table", - "fac_config.tbl", (sizeof(FAC_ConfigTblEntry_t) * FAC_CONFIG_TABLE_MAX_ENTRIES) + "fac_config.tbl", sizeof(FAC_ConfigTbl_t) }; -/************************************************************************ -** External Global Variables -*************************************************************************/ +FAC_ConfigTbl_t FAC_ConfigTbl = +{ + /** + * Attitude Roll Time Constant + * + * This defines the latency between a roll step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit s + * @min 0.4 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.4f, -/************************************************************************ -** Global Variables -*************************************************************************/ + /** + * Attitude pitch time constant + * + * This defines the latency between a pitch step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit s + * @min 0.2 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.4f, -/** -** \brief Default FAC config table data -*/ -FAC_ConfigTblEntry_t FAC_ConfigTbl[FAC_CONFIG_TABLE_MAX_ENTRIES] = -{ - /* Entry 1 */ - { - .iParam = 123 - - /* TODO: Add default values for Config parameters here. - ** - ** Examples: (See example of type declarations in fac_tbldefs.h) - ** .cParam = 7, - ** .cParams = "Init Str Value", - ** .ucParam = 'A', - ** .ucParams = "/dev/ttyUSB", - ** - ** .sParam = -16, - ** .sParams = {-3, -2, -1, 0, 1, 2, 3, 4}, - ** .usParam = 16, - ** .usParams = {1, 2, 3, 4, 5, 6, 7, 8}, - ** - ** .iParam = -32, - ** .iParams = {-123, -234, 0, 123, 234}, - ** .uiParam = -32, - ** .uiParams = {123, 234, 345, 456, 678}, - ** - ** .fParam = 32.1234, - ** .fParams = {-12.34, 0.0, 12.34}, - ** - ** .dParam = 123.456789, - ** .dParams = {-123.456789, 0.0, 123.456789} - */ - } -}; + /** + * Pitch rate proportional gain. + * + * This defines how much the elevator input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.08f, + + /** + * Pitch rate integrator gain. + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.5 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.02f, + + /** + * Maximum positive / up pitch rate. + * + * This limits the maximum pitch up angular rate the controller will output (in + * degrees per second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 60.0f, + + /** + * Maximum negative / down pitch rate. + * + * This limits the maximum pitch down up angular rate the controller will + * output (in degrees per second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 60.0f, + + /** + * Pitch rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.4f, + + /** + * Roll rate proportional Gain + * + * This defines how much the aileron input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.05f, + + /** + * Roll rate integrator Gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.2 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.01f, + + /** + * Roll integrator anti-windup + * + * The portion of the integrator part in the control surface deflection is limited to this value. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.2f, + + /** + * Maximum roll rate + * + * This limits the maximum roll rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 70.0f, + + /** + * Yaw rate proportional gain + * + * This defines how much the rudder input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.05f, + + /** + * Yaw rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.0 + * @max 50.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Yaw rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.2f, + + /** + * Maximum yaw rate + * + * This limits the maximum yaw rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Roll control to yaw control feedforward gain. + * + * This gain can be used to counteract the "adverse yaw" effect for fixed wings. + * When the plane enters a roll it will tend to yaw the nose out of the turn. + * This gain enables the use of a yaw actuator (rudder, airbrakes, ...) to counteract + * this effect. + * + * @min 0.0 + * @decimal 1 + * @increment 0.01 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Enable wheel steering controller + * + * @boolean + * @group FW Attitude Control + */ + 0, -/************************************************************************ -** Local Variables -*************************************************************************/ + /** + * Wheel steering rate proportional gain + * + * This defines how much the wheel steering input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.5f, -/************************************************************************ -** Local Function Prototypes -*************************************************************************/ + /** + * Wheel steering rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.5 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.1f, -/************************************************************************ -** Function Definitions -*************************************************************************/ + /** + * Wheel steering rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 1.0f, -/************************/ -/* End of File Comment */ -/************************/ + /** + * Maximum wheel steering rate + * + * This limits the maximum wheel steering rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Roll rate feed forward + * + * Direct feed forward from rate setpoint to control surface output. Use this + * to obtain a tigher response of the controller without introducing + * noise amplification. + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.5f, + + /** + * Pitch rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.5f, + + /** + * Yaw rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.3f, + + /** + * Wheel steering rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.2f, + + /** + * Minimal speed for yaw coordination + * + * For airspeeds above this value, the yaw rate is calculated for a coordinated + * turn. Set to a very high value to disable. + * + * @unit m/s + * @min 0.0 + * @max 1000.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 1000.0f, + + /** + * Method used for yaw coordination + * + * The param value sets the method used to calculate the yaw rate + * 0: open-loop zero lateral acceleration based on kinematic constraints + * 1: closed-loop: try to reduce lateral acceleration to 0 by measuring the acceleration + * + * @min 0 + * @max 1 + * @value 0 open-loop + * @value 1 closed-loop + * @group FW Attitude Control + */ + 0, + + /** + * Roll setpoint offset + * + * An airframe specific offset of the roll setpoint in degrees, the value is + * added to the roll setpoint and should correspond to the typical cruise speed + * of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Pitch setpoint offset + * + * An airframe specific offset of the pitch setpoint in degrees, the value is + * added to the pitch setpoint and should correspond to the typical cruise + * speed of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Max manual roll + * + * Max roll for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 45.0f, + + /** + * Max manual pitch + * + * Max pitch for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 45.0f, + + /** + * Scale factor for flaps + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 1.0f, + + /** + * Scale factor for flaperons + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Disable airspeed sensor + * + * For small wings or VTOL without airspeed sensor this parameter can be used to + * enable flying without an airspeed reading + * + * @boolean + * @group FW Attitude Control + */ + 0, + + /** + * Manual roll scale + * + * Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 1.0f, + + /** + * Manual pitch scale + * + * Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 1.0f, + + /** + * Manual yaw scale + * + * Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 1.0f, + + /** + * Whether to scale throttle by battery power level + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The fixed wing + * should constantly behave as if it was fully charged with reduced max thrust + * at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group FW Attitude Control + */ + 0, + + /** + * Acro body x max rate. + * + * This is the rate the controller is trying to achieve if the user applies full roll + * stick input in acro mode. + * + * @min 45 + * @max 720 + * @unit degrees + * @group FW Attitude Control + */ + 90, + + /** + * Acro body y max rate. + * + * This is the body y rate the controller is trying to achieve if the user applies full pitch + * stick input in acro mode. + * + * @min 45 + * @max 720 + * @unit degrees + * @group FW Attitude Control + */ + 90, + + /** + * Acro body z max rate. + * + * This is the body z rate the controller is trying to achieve if the user applies full yaw + * stick input in acro mode. + * + * @min 10 + * @max 180 + * @unit degrees + * @group FW Attitude Control + */ + 45, + + /** + * Threshold for Rattitude mode + * + * Manual input needed in order to override attitude control rate setpoints + * and instead pass manual stick inputs as rate setpoints + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 0.8f + +}; diff --git a/apps/fac/fsw/unit_test/fac_test_utils.c b/apps/fac/fsw/unit_test/fac_test_utils.c index 2d40c8306..da7841024 100644 --- a/apps/fac/fsw/unit_test/fac_test_utils.c +++ b/apps/fac/fsw/unit_test/fac_test_utils.c @@ -51,7 +51,7 @@ /* * Config table for testing */ -FAC_ConfigTblEntry_t FAC_configtable = { +FAC_ConfigTbl_t FAC_configtable = { 1 /* iParam*/ }; diff --git a/config/shared/apps/fac/tables/fac_config.c b/config/shared/apps/fac/tables/fac_config.c index 537a980f0..dd1bbc1b0 100644 --- a/config/shared/apps/fac/tables/fac_config.c +++ b/config/shared/apps/fac/tables/fac_config.c @@ -1,22 +1,6 @@ - -/************************************************************************ -** Pragmas -*************************************************************************/ - -/************************************************************************ -** Includes -*************************************************************************/ #include "cfe_tbl_filedef.h" #include "fac_tbldefs.h" -/************************************************************************ -** Local Defines -*************************************************************************/ - -/************************************************************************ -** Local Structure Definitions -*************************************************************************/ - /** ** \brief The cFE FAC config table definition. ** @@ -33,65 +17,611 @@ static CFE_TBL_FileDef_t CFE_TBL_FileDef = { "FAC_ConfigTbl", "FAC.CONFIG_TBL", "FAC default config table", - "fac_config.tbl", (sizeof(FAC_ConfigTblEntry_t) * FAC_CONFIG_TABLE_MAX_ENTRIES) + "fac_config.tbl", sizeof(FAC_ConfigTbl_t) }; -/************************************************************************ -** External Global Variables -*************************************************************************/ +FAC_ConfigTbl_t FAC_ConfigTbl = +{ + /** + * Attitude Roll Time Constant + * + * This defines the latency between a roll step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit s + * @min 0.4 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.4f, -/************************************************************************ -** Global Variables -*************************************************************************/ + /** + * Attitude pitch time constant + * + * This defines the latency between a pitch step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit s + * @min 0.2 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.4f, -/** -** \brief Default FAC config table data -*/ -FAC_ConfigTblEntry_t FAC_ConfigTbl[FAC_CONFIG_TABLE_MAX_ENTRIES] = -{ - /* Entry 1 */ - { - .iParam = 123 - - /* TODO: Add default values for Config parameters here. - ** - ** Examples: (See example of type declarations in fac_tbldefs.h) - ** .cParam = 7, - ** .cParams = "Init Str Value", - ** .ucParam = 'A', - ** .ucParams = "/dev/ttyUSB", - ** - ** .sParam = -16, - ** .sParams = {-3, -2, -1, 0, 1, 2, 3, 4}, - ** .usParam = 16, - ** .usParams = {1, 2, 3, 4, 5, 6, 7, 8}, - ** - ** .iParam = -32, - ** .iParams = {-123, -234, 0, 123, 234}, - ** .uiParam = -32, - ** .uiParams = {123, 234, 345, 456, 678}, - ** - ** .fParam = 32.1234, - ** .fParams = {-12.34, 0.0, 12.34}, - ** - ** .dParam = 123.456789, - ** .dParams = {-123.456789, 0.0, 123.456789} - */ - } -}; + /** + * Pitch rate proportional gain. + * + * This defines how much the elevator input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.08f, + + /** + * Pitch rate integrator gain. + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.5 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.02f, + + /** + * Maximum positive / up pitch rate. + * + * This limits the maximum pitch up angular rate the controller will output (in + * degrees per second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 60.0f, + + /** + * Maximum negative / down pitch rate. + * + * This limits the maximum pitch down up angular rate the controller will + * output (in degrees per second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 60.0f, + + /** + * Pitch rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.4f, + + /** + * Roll rate proportional Gain + * + * This defines how much the aileron input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.05f, + + /** + * Roll rate integrator Gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.2 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.01f, + + /** + * Roll integrator anti-windup + * + * The portion of the integrator part in the control surface deflection is limited to this value. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.2f, + + /** + * Maximum roll rate + * + * This limits the maximum roll rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 70.0f, + + /** + * Yaw rate proportional gain + * + * This defines how much the rudder input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.05f, + + /** + * Yaw rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.0 + * @max 50.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Yaw rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.2f, + + /** + * Maximum yaw rate + * + * This limits the maximum yaw rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Roll control to yaw control feedforward gain. + * + * This gain can be used to counteract the "adverse yaw" effect for fixed wings. + * When the plane enters a roll it will tend to yaw the nose out of the turn. + * This gain enables the use of a yaw actuator (rudder, airbrakes, ...) to counteract + * this effect. + * + * @min 0.0 + * @decimal 1 + * @increment 0.01 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Enable wheel steering controller + * + * @boolean + * @group FW Attitude Control + */ + 0, -/************************************************************************ -** Local Variables -*************************************************************************/ + /** + * Wheel steering rate proportional gain + * + * This defines how much the wheel steering input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.5f, -/************************************************************************ -** Local Function Prototypes -*************************************************************************/ + /** + * Wheel steering rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.5 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ + 0.1f, -/************************************************************************ -** Function Definitions -*************************************************************************/ + /** + * Wheel steering rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 1.0f, -/************************/ -/* End of File Comment */ -/************************/ + /** + * Maximum wheel steering rate + * + * This limits the maximum wheel steering rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Roll rate feed forward + * + * Direct feed forward from rate setpoint to control surface output. Use this + * to obtain a tigher response of the controller without introducing + * noise amplification. + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.5f, + + /** + * Pitch rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.5f, + + /** + * Yaw rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.3f, + + /** + * Wheel steering rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ + 0.2f, + + /** + * Minimal speed for yaw coordination + * + * For airspeeds above this value, the yaw rate is calculated for a coordinated + * turn. Set to a very high value to disable. + * + * @unit m/s + * @min 0.0 + * @max 1000.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 1000.0f, + + /** + * Method used for yaw coordination + * + * The param value sets the method used to calculate the yaw rate + * 0: open-loop zero lateral acceleration based on kinematic constraints + * 1: closed-loop: try to reduce lateral acceleration to 0 by measuring the acceleration + * + * @min 0 + * @max 1 + * @value 0 open-loop + * @value 1 closed-loop + * @group FW Attitude Control + */ + 0, + + /** + * Roll setpoint offset + * + * An airframe specific offset of the roll setpoint in degrees, the value is + * added to the roll setpoint and should correspond to the typical cruise speed + * of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Pitch setpoint offset + * + * An airframe specific offset of the pitch setpoint in degrees, the value is + * added to the pitch setpoint and should correspond to the typical cruise + * speed of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Max manual roll + * + * Max roll for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 45.0f, + + /** + * Max manual pitch + * + * Max pitch for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ + 45.0f, + + /** + * Scale factor for flaps + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 1.0f, + + /** + * Scale factor for flaperons + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 0.0f, + + /** + * Disable airspeed sensor + * + * For small wings or VTOL without airspeed sensor this parameter can be used to + * enable flying without an airspeed reading + * + * @boolean + * @group FW Attitude Control + */ + 0, + + /** + * Manual roll scale + * + * Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 1.0f, + + /** + * Manual pitch scale + * + * Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 1.0f, + + /** + * Manual yaw scale + * + * Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 1.0f, + + /** + * Whether to scale throttle by battery power level + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The fixed wing + * should constantly behave as if it was fully charged with reduced max thrust + * at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group FW Attitude Control + */ + 0, + + /** + * Acro body x max rate. + * + * This is the rate the controller is trying to achieve if the user applies full roll + * stick input in acro mode. + * + * @min 45 + * @max 720 + * @unit degrees + * @group FW Attitude Control + */ + 90, + + /** + * Acro body y max rate. + * + * This is the body y rate the controller is trying to achieve if the user applies full pitch + * stick input in acro mode. + * + * @min 45 + * @max 720 + * @unit degrees + * @group FW Attitude Control + */ + 90, + + /** + * Acro body z max rate. + * + * This is the body z rate the controller is trying to achieve if the user applies full yaw + * stick input in acro mode. + * + * @min 10 + * @max 180 + * @unit degrees + * @group FW Attitude Control + */ + 45, + + /** + * Threshold for Rattitude mode + * + * Manual input needed in order to override attitude control rate setpoints + * and instead pass manual stick inputs as rate setpoints + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ + 0.8f + +}; diff --git a/config/shared/inc/px4_msgs.h b/config/shared/inc/px4_msgs.h new file mode 100644 index 000000000..b51900ad6 --- /dev/null +++ b/config/shared/inc/px4_msgs.h @@ -0,0 +1,1382 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2017 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef _PX4_MSGS_H_ +#define _PX4_MSGS_H_ + +#include "cfe.h" + +#define PX4_ACTUATOR_DIRECT_MAX ( 16 ) +#define PX4_ACTUATOR_OUTPUTS_MAX ( 16 ) +#define PX4_ACTUATOR_OUTPUTS_GROUP_MAX ( 4 ) +#define PX4_ADC_CHANNEL_MAX ( 8 ) +#define PX4_ESC_CONNECTED_ESC_MAX ( 8 ) +#define PX4_ESTIMATOR_STATES_MAX ( 32 ) +#define PX4_ESTIMATOR_VIBE_MAX ( 3 ) +#define PX4_ESTIMATOR_COVARIANCES_MAX ( 28 ) +#define PX4_FENCE_MAX_VERTICES ( 16 ) +#define PX4_GPS_DUMP_DATA_MAX ( 79 ) +#define PX4_GPS_INJECT_DATA_MAX ( 182 ) +#define PX4_RC_INPUT_MAX_CHANNELS ( 18 ) +#define PX4_PWM_OUTPUT_MAX_CHANNELS ( 16 ) +#define PX4_QSHELL_REQ_MAX_STRLEN ( 100 ) +#define PX4_RC_PARAM_MAP_NCHAN ( 3 ) +#define PX4_RC_PARAM_MAP_ID_LEN ( 51 ) +#define PX4_SAT_INFO_MAX_SATELLITES ( 20 ) +#define PX4_NUM_MOTOR_OUTPUTS ( 8 ) +#define PX4_ADSB_CALLSIGN_LEN ( 8 ) +#define PX4_OPTICAL_FLOW_FRAME_SIZE ( 4096 ) + +typedef enum +{ + PX4_GPS_NONE0_FIX = 0, + PX4_GPS_NONE1_FIX = 1, + PX4_GPS_2D_FIX = 2, + PX4_GPS_3D_FIX = 3, + PX4_GPS_DGPS_FIX = 4, + PX4_GPS_RTK_FIX = 5 + +} PX4_GpsFixType_t; + +typedef enum +{ + PX4_DISTANCE_SENSOR_LASER = 0, + PX4_DISTANCE_SENSOR_ULTRASOUND = 1, + PX4_DISTANCE_SENSOR_INFRARED = 2, + PX4_DISTANCE_SENSOR_RADAR = 3 +} PX4_DistanceSensorType_t; + +typedef enum +{ + PX4_ACTUATOR_CONTROL_ROLL = 0, + PX4_ACTUATOR_CONTROL_PITCH = 1, + PX4_ACTUATOR_CONTROL_YAW = 2, + PX4_ACTUATOR_CONTROL_THROTTLE = 3, + PX4_ACTUATOR_CONTROL_FLAPS = 4, + PX4_ACTUATOR_CONTROL_SPOILERS = 5, + PX4_ACTUATOR_CONTROL_AIRBRAKES = 6, + PX4_ACTUATOR_CONTROL_LANDING_GEAR = 7, + PX4_ACTUATOR_CONTROL_COUNT = 8 +} PX4_ActuatorControlIndex_t; + +typedef enum +{ + PX4_ACTUATOR_CONTROL_GROUP_ATTITUDE = 0, + PX4_ACTUATOR_CONTROL_GROUP_ATTITUDE_ALTERNATE = 1, + PX4_ACTUATOR_CONTROL_GROUP_COUNT = 4 +} PX4_ActuatorControlGroupIndex_t; + +typedef enum +{ + PX4_BATTERY_WARNING_NONE = 0, + PX4_BATTERY_WARNING_LOW = 1, + PX4_BATTERY_WARNING_CRITICAL = 2, + PX4_BATTERY_WARNING_EMERGENCY = 3 +} PX4_BatteryWarningSeverity_t; + +typedef enum +{ + PX4_COMMANDER_MAIN_STATE_MANUAL = 0, + PX4_COMMANDER_MAIN_STATE_ALTCTL = 1, + PX4_COMMANDER_MAIN_STATE_POSCTL = 2, + PX4_COMMANDER_MAIN_STATE_AUTO_MISSION = 3, + PX4_COMMANDER_MAIN_STATE_AUTO_LOITER = 4, + PX4_COMMANDER_MAIN_STATE_AUTO_RTL = 5, + PX4_COMMANDER_MAIN_STATE_ACRO = 6, + PX4_COMMANDER_MAIN_STATE_OFFBOARD = 7, + PX4_COMMANDER_MAIN_STATE_STAB = 8, + PX4_COMMANDER_MAIN_STATE_RATTITUDE = 9, + PX4_COMMANDER_MAIN_STATE_AUTO_TAKEOFF = 10, + PX4_COMMANDER_MAIN_STATE_AUTO_LAND = 11, + PX4_COMMANDER_MAIN_STATE_AUTO_FOLLOW_TARGET = 12, + PX4_COMMANDER_MAIN_STATE_AUTO_PRECLAND = 13, + PX4_COMMANDER_MAIN_STATE_DESCEND = 14, + PX4_COMMANDER_MAIN_STATE_TERMINATION = 15, + PX4_COMMANDER_MAIN_STATE_AUTO_OFFBOARD = 16, + PX4_COMMANDER_MAIN_STATE_AUTO_RTGS = 17, + PX4_COMMANDER_MAIN_STATE_AUTO_RATTITUDE = 18, + PX4_COMMANDER_MAIN_STATE_AUTO_LANDENGFAIL = 19, + PX4_COMMANDER_MAIN_STATE_AUTO_LANDGPSFAIL = 20, + PX4_COMMANDER_MAIN_STATE_AUTO_RCRECOVER = 21, + PX4_COMMANDER_MAIN_STATE_MAX = 22 +} PX4_CommanderMainState_t; + +typedef enum +{ + PX4_AIRSPEED_MODE_MEAS = 0, + PX4_AIRSPEED_MODE_EST = 1, + PX4_AIRSPEED_MODE_DISABLED = 2 +} PX4_AirspeedMode_t; + +typedef enum +{ + PX4_ESC_VENDOR_GENERIC = 0, + PX4_ESC_VENDOR_MIKROKOPTER = 1, + PX4_ESC_VENDOR_GRAUPNER_HOTT = 2, + PX4_ESC_VENDOR_TAP = 3 +} PX4_EscVendor_t; + +typedef enum +{ + PX4_ESC_CONNECTION_TYPE_PPM = 0, + PX4_ESC_CONNECTION_TYPE_SERIAL = 1, + PX4_ESC_CONNECTION_TYPE_ONESHOOT = 2, + PX4_ESC_CONNECTION_TYPE_I2C = 3, + PX4_ESC_CONNECTION_TYPE_CAN = 4 +} PX4_EscConnectionType_t; + +typedef enum +{ + PX4_GEOFENCE_ACTION_NONE = 0, + PX4_GEOFENCE_ACTION_WARN = 1, + PX4_GEOFENCE_ACTION_LOITER = 2, + PX4_GEOFENCE_ACTION_RTL = 3, + PX4_GEOFENCE_ACTION_TERMINATE = 4 +} PX4_GeofenceAction_t; + +typedef enum +{ + PX4_MAGNETOMETER_MODE_NORMAL = 0, + PX4_MAGNETOMETER_MODE_POSITIVE_BIAS = 1, + PX4_MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 +} PX4_MagnetometerMode_t; + +typedef enum +{ + PX4_RC_INPUT_SOURCE_UNKNOWN = 0, + PX4_RC_INPUT_SOURCE_PX4FMU_PPM = 1, + PX4_RC_INPUT_SOURCE_PX4IO_PPM = 2, + PX4_RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3, + PX4_RC_INPUT_SOURCE_PX4IO_SBUS = 4, + PX4_RC_INPUT_SOURCE_PX4IO_ST24 = 5, + PX4_RC_INPUT_SOURCE_MAVLINK = 6, + PX4_RC_INPUT_SOURCE_QURT = 7, + PX4_RC_INPUT_SOURCE_PX4FMU_SPEKTRUM = 8, + PX4_RC_INPUT_SOURCE_PX4FMU_SBUS = 9, + PX4_RC_INPUT_SOURCE_PX4FMU_ST24 = 10, + PX4_RC_INPUT_SOURCE_PX4FMU_SUMD = 11, + PX4_RC_INPUT_SOURCE_PX4FMU_DSM = 12, + PX4_RC_INPUT_SOURCE_PX4IO_SUMD = 13 +} PX4_RcInputSource_t; + +typedef enum +{ + PX4_SWITCH_POS_NONE = 0, + PX4_SWITCH_POS_ON = 1, + PX4_SWITCH_POS_MIDDLE = 2, + PX4_SWITCH_POS_OFF = 3 +} PX4_SwitchPos_t; + +typedef enum +{ + PX4_MANUAL_CONTROL_SOURCE_RC = 1, + PX4_MANUAL_CONTROL_SOURCE_MAVLINK_0 = 2, + PX4_MANUAL_CONTROL_SOURCE_MAVLINK_1 = 3, + PX4_MANUAL_CONTROL_SOURCE_MAVLINK_2 = 4, + PX4_MANUAL_CONTROL_SOURCE_MAVLINK_3 = 5 +} PX4_ManualControlDataSource_t; + + +typedef enum +{ + PX4_MODE_SLOT_NONE = -1, + PX4_MODE_SLOT_1 = 0, + PX4_MODE_SLOT_2 = 1, + PX4_MODE_SLOT_3 = 2, + PX4_MODE_SLOT_4 = 3, + PX4_MODE_SLOT_5 = 4, + PX4_MODE_SLOT_6 = 5, + PX4_MODE_SLOT_MAX = 6 +} PX4_ModeSlot_t; + +typedef enum +{ + PX4_SETPOINT_TYPE_POSITION = 0, + PX4_SETPOINT_TYPE_VELOCITY = 1, + PX4_SETPOINT_TYPE_LOITER = 2, + PX4_SETPOINT_TYPE_TAKEOFF = 3, + PX4_SETPOINT_TYPE_LAND = 4, + PX4_SETPOINT_TYPE_IDLE = 5, + PX4_SETPOINT_TYPE_OFFBOARD = 6, + PX4_SETPOINT_TYPE_FOLLOW_TARGET = 7 +} PX4_SetpointType_t; + +typedef enum +{ + PX4_VELOCITY_FRAME_LOCAL_NED = 1, + PX4_VELOCITY_FRAME_BODY_NED = 8 +} PX4_VelocityFrameType_t; + +typedef enum +{ + PX4_RC_CHANNELS_FUNCTION_THROTTLE = 0, + PX4_RC_CHANNELS_FUNCTION_ROLL = 1, + PX4_RC_CHANNELS_FUNCTION_PITCH = 2, + PX4_RC_CHANNELS_FUNCTION_YAW = 3, + PX4_RC_CHANNELS_FUNCTION_MODE = 4, + PX4_RC_CHANNELS_FUNCTION_RETURN = 5, + PX4_RC_CHANNELS_FUNCTION_POSCTL = 6, + PX4_RC_CHANNELS_FUNCTION_LOITER = 7, + PX4_RC_CHANNELS_FUNCTION_OFFBOARD = 8, + PX4_RC_CHANNELS_FUNCTION_ACRO = 9, + PX4_RC_CHANNELS_FUNCTION_FLAPS = 10, + PX4_RC_CHANNELS_FUNCTION_AUX_1 = 11, + PX4_RC_CHANNELS_FUNCTION_AUX_2 = 12, + PX4_RC_CHANNELS_FUNCTION_AUX_3 = 13, + PX4_RC_CHANNELS_FUNCTION_AUX_4 = 14, + PX4_RC_CHANNELS_FUNCTION_AUX_5 = 15, + PX4_RC_CHANNELS_FUNCTION_PARAM_1 = 16, + PX4_RC_CHANNELS_FUNCTION_PARAM_2 = 17, + PX4_RC_CHANNELS_FUNCTION_PARAM_3 = 18, + PX4_RC_CHANNELS_FUNCTION_RATTITUDE = 19, + PX4_RC_CHANNELS_FUNCTION_KILLSWITCH = 20, + PX4_RC_CHANNELS_FUNCTION_TRANSITION = 21, + PX4_RC_CHANNELS_FUNCTION_GEAR = 22, + PX4_RC_CHANNELS_FUNCTION_ARMSWITCH = 23, + PX4_RC_CHANNELS_FUNCTION_STAB = 24, + PX4_RC_CHANNELS_FUNCTION_MAN = 25, + PX4_RC_CHANNELS_FUNCTION_ALTCTL = 26, + PX4_RC_CHANNELS_FUNCTION_COUNT = 27 +} PX4_RcChannelFunction_t; + +typedef enum +{ + PX4_SUBSYSTEM_TYPE_GYRO = 0b00000000000000000001, /* 1 */ + PX4_SUBSYSTEM_TYPE_ACC = 0b00000000000000000010, /* 2 */ + PX4_SUBSYSTEM_TYPE_MAG = 0b00000000000000000100, /* 4 */ + PX4_SUBSYSTEM_TYPE_ABSPRESSURE = 0b00000000000000001000, /* 8 */ + PX4_SUBSYSTEM_TYPE_DIFFPRESSURE = 0b00000000000000010000, /* 16 */ + PX4_SUBSYSTEM_TYPE_GPS = 0b00000000000000100000, /* 32 */ + PX4_SUBSYSTEM_TYPE_OPTICALFLOW = 0b00000000000001000000, /* 64 */ + PX4_SUBSYSTEM_TYPE_CVPOSITION = 0b00000000000010000000, /* 128 */ + PX4_SUBSYSTEM_TYPE_LASERPOSITION = 0b00000000000100000000, /* 256 */ + PX4_SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 0b00000000001000000000, /* 512 */ + PX4_SUBSYSTEM_TYPE_ANGULARRATECONTROL = 0b00000000010000000000, /* 1024 */ + PX4_SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 0b00000000100000000000, /* 2048 */ + PX4_SUBSYSTEM_TYPE_YAWPOSITION = 0b00000001000000000000, /* 4096 */ + /* Skip 8192 */ + PX4_SUBSYSTEM_TYPE_ALTITUDECONTROL = 0b00000100000000000000, /* 16384 */ + PX4_SUBSYSTEM_TYPE_POSITIONCONTROL = 0b00001000000000000000, /* 32768 */ + PX4_SUBSYSTEM_TYPE_MOTORCONTROL = 0b00010000000000000000, /* 65536 */ + PX4_SUBSYSTEM_TYPE_RANGEFINDER = 0b00100000000000000000 /* 131072 */ +} PX4_SubsystemType_t; + +typedef enum +{ + PX4_TECS_MODE_NORMAL = 0, + PX4_TECS_MODE_UNDERSPEED = 1, + PX4_TECS_MODE_TAKEOFF = 2, + PX4_TECS_MODE_LAND = 3, + PX4_TECS_MODE_LAND_THROTTLELIM = 4, + PX4_TECS_MODE_BAD_DESCENT = 5, + PX4_TECS_MODE_CLIMBOUT = 6 +} PX4_TecsMode_t; + +typedef enum +{ + PX4_TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, + PX4_TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1, + PX4_TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2, + PX4_TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3, + PX4_TELEMETRY_STATUS_RADIO_TYPE_USB = 4 +} PX4_TelemetryStatusRadioType_t; + +typedef enum +{ + PX4_ADSB_ALTITUDE_TYPE_PRESSURE_QNH = 0, + PX4_ADSB_ALTITUDE_TYPE_GEOMETRIC = 1 +} PX4_AdsbAltitudeType_t; + +typedef enum +{ + PX4_ADSB_EMITTER_TYPE_NO_INFO = 0, + PX4_ADSB_EMITTER_TYPE_LIGHT = 1, + PX4_ADSB_EMITTER_TYPE_SMALL = 2, + PX4_ADSB_EMITTER_TYPE_LARGE = 3, + PX4_ADSB_EMITTER_TYPE_HIGH_VORTEX_LARGE = 4, + PX4_ADSB_EMITTER_TYPE_HEAVY = 5, + PX4_ADSB_EMITTER_TYPE_HIGHLY_MANUV = 6, + PX4_ADSB_EMITTER_TYPE_ROTOCRAFT = 7, + PX4_ADSB_EMITTER_TYPE_UNASSIGNED = 8, + PX4_ADSB_EMITTER_TYPE_GLIDER = 9, + PX4_ADSB_EMITTER_TYPE_LIGHTER_AIR = 10, + PX4_ADSB_EMITTER_TYPE_PARACHUTE = 11, + PX4_ADSB_EMITTER_TYPE_ULTRA_LIGHT = 12, + PX4_ADSB_EMITTER_TYPE_UNASSIGNED2 = 13, + PX4_ADSB_EMITTER_TYPE_UAV = 14, + PX4_ADSB_EMITTER_TYPE_SPACE = 15, + PX4_ADSB_EMITTER_TYPE_UNASSGINED3 = 16, + PX4_ADSB_EMITTER_TYPE_EMERGENCY_SURFACE = 17, + PX4_ADSB_EMITTER_TYPE_SERVICE_SURFACE = 18, + PX4_ADSB_EMITTER_TYPE_POINT_OBSTACLE = 19 +} PX4_AdsbEmitterType_t; + +typedef enum +{ + PX4_ADSB_FLAGS_VALID_COORDS = 0b00000001, /* 1 */ + PX4_ADSB_FLAGS_VALID_ALTITUDE = 0b00000010, /* 2 */ + PX4_ADSB_FLAGS_VALID_HEADING = 0b00000100, /* 4 */ + PX4_ADSB_FLAGS_VALID_VELOCITY = 0b00001000, /* 8 */ + PX4_ADSB_FLAGS_VALID_CALLSIGN = 0b00010000, /* 16 */ + PX4_ADSB_FLAGS_VALID_SQUAWK = 0b00100000, /* 32 */ + PX4_ADSB_FLAGS_SIMULATED = 0b01000000 /* 64 */ +} PX4_AdsbFlags_t; + +typedef enum +{ + PX4_VEHICLE_COMMAND_RESULT_ACCEPTED = 0, + PX4_VEHICLE_COMMAND_RESULT_TEMPORARILY_REJECTED = 1, + PX4_VEHICLE_COMMAND_RESULT_DENIED = 2, + PX4_VEHICLE_COMMAND_RESULT_UNSUPPORTED = 3, + PX4_VEHICLE_COMMAND_RESULT_FAILED = 4 +} PX4_VehicleCommandResult_t; + +typedef enum +{ + PX4_VEHICLE_CMD_CUSTOM_0 = 0, + PX4_VEHICLE_CMD_CUSTOM_1 = 1, + PX4_VEHICLE_CMD_CUSTOM_2 = 2, + PX4_VEHICLE_CMD_NAV_WAYPOINT = 16, + PX4_VEHICLE_CMD_NAV_LOITER_UNLIM = 17, + PX4_VEHICLE_CMD_NAV_LOITER_TURNS = 18, + PX4_VEHICLE_CMD_NAV_LOITER_TIME = 19, + PX4_VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20, + PX4_VEHICLE_CMD_NAV_LAND = 21, + PX4_VEHICLE_CMD_NAV_TAKEOFF = 22, + PX4_VEHICLE_CMD_NAV_LOITER_TO_ALT = 31, + PX4_VEHICLE_CMD_NAV_ROI = 80, + PX4_VEHICLE_CMD_NAV_PATHPLANNING = 81, + PX4_VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84, + PX4_VEHICLE_CMD_NAV_VTOL_LAND = 85, + PX4_VEHICLE_CMD_NAV_GUIDED_LIMITS = 90, + PX4_VEHICLE_CMD_NAV_GUIDED_MASTER = 91, + PX4_VEHICLE_CMD_NAV_GUIDED_ENABLE = 92, + PX4_VEHICLE_CMD_NAV_LAST = 95, + PX4_VEHICLE_CMD_CONDITION_DELAY = 112, + PX4_VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, + PX4_VEHICLE_CMD_CONDITION_DISTANCE = 114, + PX4_VEHICLE_CMD_CONDITION_YAW = 115, + PX4_VEHICLE_CMD_CONDITION_LAST = 159, + PX4_VEHICLE_CMD_DO_SET_MODE = 176, + PX4_VEHICLE_CMD_DO_JUMP = 177, + PX4_VEHICLE_CMD_DO_CHANGE_SPEED = 178, + PX4_VEHICLE_CMD_DO_SET_HOME = 179, + PX4_VEHICLE_CMD_DO_SET_PARAMETER = 180, + PX4_VEHICLE_CMD_DO_SET_RELAY = 181, + PX4_VEHICLE_CMD_DO_REPEAT_RELAY = 182, + PX4_VEHICLE_CMD_DO_SET_SERVO = 183, + PX4_VEHICLE_CMD_DO_REPEAT_SERVO = 184, + PX4_VEHICLE_CMD_DO_FLIGHTTERMINATION = 185, + PX4_VEHICLE_CMD_DO_GO_AROUND = 191, + PX4_VEHICLE_CMD_DO_REPOSITION = 192, + PX4_VEHICLE_CMD_DO_PAUSE_CONTINUE = 193, + PX4_VEHICLE_CMD_DO_CONTROL_VIDEO = 200, + PX4_VEHICLE_CMD_DO_DIGICAM_CONTROL = 203, + PX4_VEHICLE_CMD_DO_MOUNT_CONFIGURE = 204, + PX4_VEHICLE_CMD_DO_MOUNT_CONTROL = 205, + PX4_VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST = 206, + PX4_VEHICLE_CMD_DO_FENCE_ENABLE = 207, + PX4_VEHICLE_CMD_DO_PARACHUTE = 208, + PX4_VEHICLE_CMD_DO_INVERTED_FLIGHT = 210, + PX4_VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT = 220, + PX4_VEHICLE_CMD_DO_GUIDED_MASTER = 221, + PX4_VEHICLE_CMD_DO_GUIDED_LIMITS = 222, + PX4_VEHICLE_CMD_DO_LAST = 240, + PX4_VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, + PX4_VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242, + PX4_VEHICLE_CMD_PREFLIGHT_UAVCAN = 243, + PX4_VEHICLE_CMD_PREFLIGHT_STORAGE = 245, + PX4_VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246, + PX4_VEHICLE_CMD_OVERRIDE_GOTO = 252, + PX4_VEHICLE_CMD_MISSION_START = 300, + PX4_VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, + PX4_VEHICLE_CMD_START_RX_PAIR = 500, + PX4_VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003, + PX4_VEHICLE_CMD_DO_VTOL_TRANSITION = 3000, + PX4_VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, + PX4_VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 +} PX4_VehicleCmd_t; + +typedef enum +{ + PX4_ARMING_STATE_INIT = 0, + PX4_ARMING_STATE_STANDBY = 1, + PX4_ARMING_STATE_ARMED = 2, + PX4_ARMING_STATE_ARMED_ERROR = 3, + PX4_ARMING_STATE_STANDBY_ERROR = 4, + PX4_ARMING_STATE_REBOOT = 5, + PX4_ARMING_STATE_IN_AIR_RESTORE = 6, + PX4_ARMING_STATE_MAX = 7 +} PX4_ArmingState_t; + +typedef enum +{ + PX4_HIL_STATE_OFF = 0, + PX4_HIL_STATE_ON = 1 +} PX4_HilState_t; + +typedef enum +{ + PX4_NAVIGATION_STATE_MANUAL = 0, + PX4_NAVIGATION_STATE_ALTCTL = 1, + PX4_NAVIGATION_STATE_POSCTL = 2, + PX4_NAVIGATION_STATE_AUTO_MISSION = 3, + PX4_NAVIGATION_STATE_AUTO_LOITER = 4, + PX4_NAVIGATION_STATE_AUTO_RTL = 5, + PX4_NAVIGATION_STATE_AUTO_RCRECOVER = 6, + PX4_NAVIGATION_STATE_AUTO_RTGS = 7, + PX4_NAVIGATION_STATE_AUTO_LANDENGFAIL = 8, + PX4_NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9, + PX4_NAVIGATION_STATE_ACRO = 10, + PX4_NAVIGATION_STATE_UNUSED = 11, + PX4_NAVIGATION_STATE_DESCEND = 12, + PX4_NAVIGATION_STATE_TERMINATION = 13, + PX4_NAVIGATION_STATE_OFFBOARD = 14, + PX4_NAVIGATION_STATE_STAB = 15, + PX4_NAVIGATION_STATE_RATTITUDE = 16, + PX4_NAVIGATION_STATE_AUTO_TAKEOFF = 17, + PX4_NAVIGATION_STATE_AUTO_LAND = 18, + PX4_NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19, + PX4_NAVIGATION_STATE_AUTO_PRECLAND = 20, + PX4_NAVIGATION_STATE_MAX = 21 +} PX4_NavigationState_t; + +typedef enum +{ + PX4_RC_IN_MODE_DEFAULT = 0, + PX4_RC_IN_MODE_OFF = 1, + PX4_RC_IN_MODE_GENERATED = 2 +} PX4_RcInMode_t; + +typedef enum +{ + PX4_VEHICLE_MOUNT_MODE_RETRACT = 0, + PX4_VEHICLE_MOUNT_MODE_NEUTRAL = 1, + PX4_VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2, + PX4_VEHICLE_MOUNT_MODE_RC_TARGETING = 3, + PX4_VEHICLE_MOUNT_MODE_GPS_POINT = 4, + PX4_VEHICLE_MOUNT_MODE_ENUM_END = 5 +} PX4_VehicleMountMode_t; + +typedef enum +{ + PX4_SYSTEM_TYPE_GENERIC = 0, + PX4_SYSTEM_TYPE_FIXED_WING = 1, + PX4_SYSTEM_TYPE_QUADROTOR = 2, + PX4_SYSTEM_TYPE_COAXIAL = 3, + PX4_SYSTEM_TYPE_HELICOPTER = 4, + PX4_SYSTEM_TYPE_ANTENNA_TRACKER = 5, + PX4_SYSTEM_TYPE_GCS = 6, + PX4_SYSTEM_TYPE_AIRSHIP = 7, + PX4_SYSTEM_TYPE_FREE_BALLOON = 8, + PX4_SYSTEM_TYPE_ROCKET = 9, + PX4_SYSTEM_TYPE_GROUND_ROVER = 10, + PX4_SYSTEM_TYPE_SURFACE_BOAT = 11, + PX4_SYSTEM_TYPE_SUBMARINE = 12, + PX4_SYSTEM_TYPE_HEXAROTOR = 13, + PX4_SYSTEM_TYPE_OCTOROTOR = 14, + PX4_SYSTEM_TYPE_TRICOPTER = 15, + PX4_SYSTEM_TYPE_FLAPPING_WING = 16, + PX4_SYSTEM_TYPE_KITE = 17, + PX4_SYSTEM_TYPE_ONBOARD_CONTROLLER = 18, + PX4_SYSTEM_TYPE_VTOL_DUOROTOR = 19, + PX4_SYSTEM_TYPE_VTOL_QUADROTOR = 20, + PX4_SYSTEM_TYPE_VTOL_TILTROTOR = 21, + PX4_SYSTEM_TYPE_VTOL_RESERVED2 = 22, + PX4_SYSTEM_TYPE_VTOL_RESERVED3 = 23, + PX4_SYSTEM_TYPE_VTOL_RESERVED4 = 24, + PX4_SYSTEM_TYPE_VTOL_RESERVED5 = 25, + PX4_SYSTEM_TYPE_GIMBAL = 26, + PX4_SYSTEM_TYPE_ADSB = 27 +} PX4_SystemType_t; + +typedef enum +{ + PX4_VEHICLE_VTOL_STATE_UNDEFINED = 0, + PX4_VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1, + PX4_VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2, + PX4_VEHICLE_VTOL_STATE_MC = 3, + PX4_VEHICLE_VTOL_STATE_FW = 4 +} PX4_VehicleVtolState_t; + + +typedef enum +{ + PX4_SENSOR_ORIENTATION_NONE = 0, + PX4_SENSOR_ORIENTATION_YAW_45 = 1, + PX4_SENSOR_ORIENTATION_YAW_90 = 2, + PX4_SENSOR_ORIENTATION_YAW_135 = 3, + PX4_SENSOR_ORIENTATION_YAW_180 = 4, + PX4_SENSOR_ORIENTATION_YAW_225 = 5, + PX4_SENSOR_ORIENTATION_YAW_270 = 6, + PX4_SENSOR_ORIENTATION_YAW_315 = 7, + PX4_SENSOR_ORIENTATION_ROLL_180 = 8, + PX4_SENSOR_ORIENTATION_ROLL_180_YAW_45 = 9, + PX4_SENSOR_ORIENTATION_ROLL_180_YAW_90 = 10, + PX4_SENSOR_ORIENTATION_ROLL_180_YAW_135 = 11, + PX4_SENSOR_ORIENTATION_PITCH_180 = 12, + PX4_SENSOR_ORIENTATION_ROLL_180_YAW_225 = 13, + PX4_SENSOR_ORIENTATION_ROLL_180_YAW_270 = 14, + PX4_SENSOR_ORIENTATION_ROLL_180_YAW_315 = 15, + PX4_SENSOR_ORIENTATION_ROLL_90 = 16, + PX4_SENSOR_ORIENTATION_ROLL_90_YAW_45 = 17, + PX4_SENSOR_ORIENTATION_ROLL_90_YAW_90 = 18, + PX4_SENSOR_ORIENTATION_ROLL_90_YAW_135 = 19, + PX4_SENSOR_ORIENTATION_ROLL_270 = 20, + PX4_SENSOR_ORIENTATION_ROLL_270_YAW_45 = 21, + PX4_SENSOR_ORIENTATION_ROLL_270_YAW_90 = 22, + PX4_SENSOR_ORIENTATION_ROLL_270_YAW_135 = 23, + PX4_SENSOR_ORIENTATION_PITCH_90 = 24, + PX4_SENSOR_ORIENTATION_PITCH_270 = 25, + PX4_SENSOR_ORIENTATION_PITCH_180_YAW_90 = 26, + PX4_SENSOR_ORIENTATION_PITCH_180_YAW_270 = 27, + PX4_SENSOR_ORIENTATION_ROLL_90_PITCH_90 = 28, + PX4_SENSOR_ORIENTATION_ROLL_180_PITCH_90 = 29, + PX4_SENSOR_ORIENTATION_ROLL_270_PITCH_90 = 30, + PX4_SENSOR_ORIENTATION_ROLL_90_PITCH_180 = 31, + PX4_SENSOR_ORIENTATION_ROLL_270_PITCH_180 = 32, + PX4_SENSOR_ORIENTATION_ROLL_90_PITCH_270 = 33, + PX4_SENSOR_ORIENTATION_ROLL_180_PITCH_270 = 34, + PX4_SENSOR_ORIENTATION_ROLL_270_PITCH_270 = 35, + PX4_SENSOR_ORIENTATION_ROLL_90_PITCH_180_YAW_90 = 36, + PX4_SENSOR_ORIENTATION_ROLL_90_YAW_270 = 37, + PX4_SENSOR_ORIENTATION_ROLL_315_PITCH_315_YAW_315 = 39 +} PX4_SensorOrientation_t; + + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + boolean Armed; + boolean Prearmed; + boolean ReadyToArm; + boolean Lockdown; + boolean ManualLockdown; + boolean ForceFailsafe; + boolean InEscCalibrationMode; +} PX4_ActuatorArmedMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 SampleTime; + float Control[PX4_ACTUATOR_CONTROL_COUNT]; +} PX4_ActuatorControlsMsg_t; + + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint32 Count; + float Output[PX4_ACTUATOR_OUTPUTS_MAX]; +} PX4_ActuatorOutputsMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float IndicatedAirspeed; /* m/s */ + float TrueAirspeed; /* m/s */ + float TrueAirspeedUnfiltered; /* m/s */ + float AirTemperature; /* Celsius */ + float Confidence; +} PX4_AirspeedMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float Voltage; /* V */ + float VoltageFiltered; /* V */ + float Current; /* A */ + float CurrentFiltered; /* A */ + float Discharged; /* mAh */ + float Remaining; + float Scale; + int32 CellCount; + boolean Connected; + PX4_BatteryWarningSeverity_t Warning; +} PX4_BatteryStatusMsg_t; + + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + PX4_CommanderMainState_t MainState; +} PX4_CommanderStateMsg_t; + + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float AccX; + float AccY; + float AccZ; + float VelX; + float VelY; + float VelZ; + float PosX; + float PosY; + float PosZ; + float Airspeed; + float VelVariance[3]; + float PosVariance[3]; + float Q[4]; + float DeltaQReset[4]; + float RollRate; + float PitchRate; + float YawRate; + float HorzAccMag; + float RollRateBias; + float PitchRateBias; + float YawRateBias; + boolean AirspeedValid; + uint8 QuatResetCounter; +} PX4_ControlStateMsg_t; + + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 ErrorCount; + float DifferentialPressureRaw; + float DifferentialPressureFiltered; + float Temperature; +} PX4_DifferentialPressureMsg_t; + + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float MinDistance; + float MaxDistance; + float CurrentDistance; + float Covariance; + PX4_DistanceSensorType_t Type : 8; + uint8 ID; + PX4_SensorOrientation_t Orientation : 8; +} PX4_DistanceSensorMsg_t; + + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float States[PX4_ESTIMATOR_STATES_MAX]; + uint32 NumStates; + float Vibe[PX4_ESTIMATOR_VIBE_MAX]; + float Covariances[PX4_ESTIMATOR_COVARIANCES_MAX]; + uint16 GpsCheckFailFlags; + uint16 ControlModeFlags; + uint16 FilterFaultFlags; + uint8 NanFlags; + uint8 HealthFlags; + uint8 TimeoutFlags; +} PX4_EstimatorStatusMsg_t; + +typedef struct +{ + float Lat; + float Lon; +} PX4_FenceVertex_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint8 Len; + uint8 Flags; + char Data[PX4_GPS_INJECT_DATA_MAX]; +} PX4_GpsInjectDataMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + double Lat; + double Lon; + float Alt; + float X; + float Y; + float Z; + float Yaw; + float DirectionX; + float DirectionY; + float DirectionZ; +} PX4_HomePositionMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 LastSignal; + uint32 ChannelCount; + int32 RSSI; + uint16 RcLostFrameCount; + uint16 RcTotalFrameCount; + uint16 RcPpmFrameLength; + uint16 Values[PX4_RC_INPUT_MAX_CHANNELS]; + boolean RcFailsafe; + boolean RcLost; + PX4_RcInputSource_t InputSource; +} PX4_InputRcMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint8 Severity; + char Text[127]; +} PX4_LogMessageMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float X; + float Y; + float Z; + float R; + float Flaps; + float Aux1; + float Aux2; + float Aux3; + float Aux4; + float Aux5; + PX4_SwitchPos_t ModeSwitch; + PX4_SwitchPos_t ReturnSwitch; + PX4_SwitchPos_t RattitudeSwitch; + PX4_SwitchPos_t PosctlSwitch; + PX4_SwitchPos_t LoiterSwitch; + PX4_SwitchPos_t AcroSwitch; + PX4_SwitchPos_t OffboardSwitch; + PX4_SwitchPos_t KillSwitch; + PX4_SwitchPos_t TransitionSwitch; + PX4_SwitchPos_t GearSwitch; + PX4_SwitchPos_t ArmSwitch; + PX4_SwitchPos_t StabSwitch; + PX4_SwitchPos_t ManSwitch; + PX4_ModeSlot_t ModeSlot; + PX4_ManualControlDataSource_t DataSource; + PX4_SwitchPos_t AltctlSwitch; +} PX4_ManualControlSetpointMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float RollRateInteg; + float PitchRateInteg; + float YawRateInteg; +} PX4_McAttCtrlStatusMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + int32 DatamanID; + uint32 Count; + int32 CurrentSeq; +} PX4_MissionMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint32 InstanceCount; + uint32 SeqReached; + uint32 SeqCurrent; + uint32 SeqTotal; + uint32 ItemChangedIndex; + uint32 ItemDoJumpRemaining; + boolean Valid; + boolean Warning; + boolean Reached; + boolean Finished; + boolean Failure; + boolean StayInFailsafe; + boolean FlightTermination; + boolean ItemDoJumpChanged; +} PX4_MissionResultMsg_t; + + +typedef union +{ + struct + { + uint16 MotorPos : 1; // 0 - true when any motor has saturated in the positive direction + uint16 MotorNeg : 1; // 1 - true when any motor has saturated in the negative direction + uint16 RollPos : 1; // 2 - true when a positive roll demand change will increase saturation + uint16 RollNeg : 1; // 3 - true when a negative roll demand change will increase saturation + uint16 PitchPos : 1; // 4 - true when a positive pitch demand change will increase saturation + uint16 PitchNeg : 1; // 5 - true when a negative pitch demand change will increase saturation + uint16 YawPos : 1; // 6 - true when a positive yaw demand change will increase saturation + uint16 YawNeg : 1; // 7 - true when a negative yaw demand change will increase saturation + uint16 ThrustPos : 1; // 8 - true when a positive thrust demand change will increase saturation + uint16 ThrustNeg : 1; // 9 - true when a negative thrust demand change will increase saturation + } Flags; + uint16 Value; +} PX4_SaturationStatus_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + PX4_SaturationStatus_t SaturationStatus; +} PX4_MultirotorMotorLimitsMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float PixelFlowXIntegral; + float PixelFlowYIntegral; + float GyroXRateIntegral; + float GyroYRateIntegral; + float GyroZRateIntegral; + float GroundDistance; + uint32 IntegrationTimespan; + uint32 TimeSinceLastSonarUpdate; + uint16 FrameCountSinceLastReadout; + int16 GyroTemperature; + uint8 SensorID; + uint8 Quality; +} PX4_OpticalFlowMsg_t; + +typedef struct +{ + uint64 Timestamp; + double Lat; + double Lon; + float X; + float Y; + float Z; + float VX; + float VY; + float VZ; + float Alt; + float Yaw; + float Yawspeed; + float LoiterRadius; + float PitchMin; + float AX; + float AY; + float AZ; + float AcceptanceRadius; + float CruisingSpeed; + float CruisingThrottle; + boolean Valid; + PX4_SetpointType_t Type; + boolean PositionValid; + boolean VelocityValid; + uint8 VelocityFrame; + boolean AltValid; + boolean YawValid; + boolean DisableMcYawControl; + boolean YawspeedValid; + int8 LoiterDirection; + boolean AccelerationValid; + boolean AccelerationIsForce; +} PX4_PositionSetpoint_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + PX4_PositionSetpoint_t Previous; + PX4_PositionSetpoint_t Current; + PX4_PositionSetpoint_t Next; +} PX4_PositionSetpointTripletMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 TimestampLastValid; + float Channels[PX4_RC_INPUT_MAX_CHANNELS]; + uint32 FrameDropCount; + uint8 ChannelCount; + PX4_RcChannelFunction_t Function[PX4_RC_CHANNELS_FUNCTION_COUNT]; + uint8 RSSI; + boolean SignalLost; +} PX4_RcChannelsMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + boolean SafetySwitchAvailable; + boolean SafetyOff; +} PX4_SafetyMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint8 Count; + uint8 SVID[PX4_SAT_INFO_MAX_SATELLITES]; + uint8 Used[PX4_SAT_INFO_MAX_SATELLITES]; + uint8 Elevation[PX4_SAT_INFO_MAX_SATELLITES]; + uint8 Azimuth[PX4_SAT_INFO_MAX_SATELLITES]; + uint8 SNR[PX4_SAT_INFO_MAX_SATELLITES]; +} PX4_SatelliteInfoMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 IntegralDt; + uint64 ErrorCount; + float X; + float Y; + float Z; + float XIntegral; + float YIntegral; + float ZIntegral; + float Temperature; + float Range_m_s2; + float Scaling; + uint32 DeviceID; + int16 XRaw; + int16 YRaw; + int16 ZRaw; + int16 TemperatureRaw; +} PX4_SensorAccelMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint32 ErrorCount; + float Pressure; + float Altitude; + float Temperature; +} PX4_SensorBaroMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float GyroRad[3]; + float GyroIntegralDt; + uint64 AccTimestamp; + boolean AccInvalid; + float Acc[3]; + float AccIntegralDt; + uint64 MagTimestamp; + boolean MagInvalid; + float Mag[3]; + uint64 BaroTimestamp; + boolean BaroInvalid; + float BaroAlt; + float BaroTemp; +} PX4_SensorCombinedMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 IntegralDt; + uint64 ErrorCount; + float X; + float Y; + float Z; + float XIntegral; + float YIntegral; + float ZIntegral; + float Temperature; + float Range; + float Scaling; + uint32 DeviceID; + int16 XRaw; + int16 YRaw; + int16 ZRaw; + int16 TemperatureRaw; +} PX4_SensorGyroMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 ErrorCount; + float X; + float Y; + float Z; + float Range; + float Scaling; + float Temperature; + uint32 DeviceID; + int16 XRaw; + int16 YRaw; + int16 ZRaw; +} PX4_SensorMagMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + PX4_SubsystemType_t SubsystemType; + boolean Present; + boolean Enabled; + boolean Ok; +} PX4_SubsystemInfoMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 HeartbeatTime; + uint64 TelemTime; + uint16 RxErrors; + uint16 Fixed; + PX4_TelemetryStatusRadioType_t Type; + uint8 RSSI; + uint8 RemoteRSSI; + uint8 Noise; + uint8 RemoteNoise; + uint8 TxBuf; + uint8 SystemID; + uint8 ComponentID; +} PX4_TelemetryStatusMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float RollSpeed; + float PitchSpeed; + float YawSpeed; + float Q[4]; +} PX4_VehicleAttitudeMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float RollBody; + float PitchBody; + float YawBody; + float YawSpMoveRate; + float Q_D[4]; + boolean Q_D_Valid; + float Thrust; + boolean RollResetIntegral; + boolean PitchResetIntegral; + boolean YawResetIntegral; + boolean FwControlYaw; + boolean DisableMcYawControl; + boolean ApplyFlaps; + float LandingGear; +} PX4_VehicleAttitudeSetpointMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint16 Command; + PX4_VehicleCommandResult_t Result; +} PX4_VehicleCommandAckMsg_t; + + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + double Param5; + double Param6; + float Param1; + float Param2; + float Param3; + float Param4; + float Param7; + PX4_VehicleCmd_t Command; + uint32 TargetSystem; + uint32 TargetComponent; + uint32 SourceSystem; + uint32 SourceComponent; + uint8 Confirmation; +} PX4_VehicleCommandMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + boolean Armed; + boolean ExternalManualOverrideOk; + boolean SystemHilEnabled; + boolean ControlManualEnabled; + boolean ControlAutoEnabled; + boolean ControlOffboardEnabled; + boolean ControlRatesEnabled; + boolean ControlAttitudeEnabled; + boolean ControlRattitudeEnabled; + boolean ControlForceEnabled; + boolean ControlAccelerationEnabled; + boolean ControlVelocityEnabled; + boolean ControlPositionEnabled; + boolean ControlAltitudeEnabled; + boolean ControlClimbRateEnabled; + boolean ControlTerminationEnabled; + boolean ControlFixedHdgEnabled; +} PX4_VehicleControlModeMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 TimeUtcUsec; + double Lat; + double Lon; + float Alt; + double DeltaLatLon[2]; + float DeltaAlt; + uint8 LatLonResetCounter; + uint8 AltResetCounter; + float VelN; + float VelE; + float VelD; + float Yaw; + float EpH; + float EpV; + float EvH; + float EvV; + float TerrainAlt; + float PressureAlt; + boolean TerrainAltValid; + boolean DeadReckoning; +} PX4_VehicleGlobalPositionMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float VX; + float VY; + float VZ; +} PX4_VehicleGlobalVelocitySetpointMsg_t; + + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 TimeUtcUsec; + int32 Lat; + int32 Lon; + int32 Alt; + int32 AltEllipsoid; + float SVariance; + float CVariance; + float EpH; + float EpV; + float HDOP; + float VDOP; + int32 NoisePerMs; + int32 JammingIndicator; + float Vel_m_s; + float Vel_n_m_s; + float Vel_e_m_s; + float Vel_d_m_s; + float COG; + int32 TimestampTimeRelative; + PX4_GpsFixType_t FixType; + boolean VelNedValid; + uint8 SatellitesUsed; +} PX4_VehicleGpsPositionMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float AltMax; + boolean Landed; + boolean Freefall; + boolean GroundContact; +} PX4_VehicleLandDetectedMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint64 RefTimestamp; + double RefLat; + double RefLon; + uint64 SurfaceBottomTimestamp; + float X; + float Y; + float Z; + float Delta_XY[2]; + float Delta_Z; + float VX; + float VY; + float VZ; + float Delta_VXY[2]; + float Delta_VZ; + float AX; + float AY; + float AZ; + float Yaw; + float RefAlt; + float DistBottom; + float DistBottomRate; + float EpH; + float EpV; + float EvH; + float EvV; + uint8 EstimatorType; + boolean XY_Valid; + boolean Z_Valid; + boolean V_XY_Valid; + boolean V_Z_Valid; + uint8 XY_ResetCounter; + uint8 Z_ResetCounter; + uint8 VXY_ResetCounter; + uint8 VZ_ResetCounter; + boolean XY_Global; + boolean Z_Global; + boolean DistBottomValid; +} PX4_VehicleLocalPositionMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float X; + float Y; + float Z; + float Yaw; + float VX; + float VY; + float VZ; + float AccX; + float AccY; + float AccZ; +} PX4_VehicleLocalPositionSetpointMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float Roll; + float Pitch; + float Yaw; + float Thrust; +} PX4_VehicleRatesSetpointMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint32 SystemID; + uint32 ComponentID; + uint32 OnboardControlSensorsPresent; + uint32 OnboardControlSensorsEnabled; + uint32 OnboardControlSensorsHealth; + PX4_NavigationState_t NavState; + PX4_ArmingState_t ArmingState; + PX4_HilState_t HilState; + boolean Failsafe; + PX4_SystemType_t SystemType; + boolean IsRotaryWing; + boolean IsVtol; + boolean VtolFwPermanentStab; + boolean InTransitionMode; + boolean RcSignalLost; + PX4_RcInMode_t RcInputMode; + boolean DataLinkLost; + uint8 DataLinkLostCounter; + boolean EngineFailure; + boolean EngineFailureCmd; + boolean MissionFailure; +} PX4_VehicleStatusMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + float gyro_offset_0[3]; + float gyro_scale_0[3]; + float gyro_offset_1[3]; + float gyro_scale_1[3]; + float gyro_offset_2[3]; + float gyro_scale_2[3]; + float accel_offset_0[3]; + float accel_scale_0[3]; + float accel_offset_1[3]; + float accel_scale_1[3]; + float accel_offset_2[3]; + float accel_scale_2[3]; + float baro_offset_0; + float baro_scale_0; + float baro_offset_1; + float baro_scale_1; + float baro_offset_2; + float baro_scale_2; + uint8 selected_gyro_instance; + uint8 selected_accel_instance; + uint8 selected_baro_instance; + uint8 gyro_mapping[3]; + uint8 accel_mapping[3]; + uint8 baro_mapping[3]; +} PX4_SensorCorrectionMsg_t; + + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint8 LedMask; + uint8 Color; + uint8 Mode; + uint8 NumBlinks; + uint8 Priority; +} PX4_LedControlMsg_t; + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint64 Timestamp; + uint8 Frame[4096]; +} PX4_OpticalFlowFrameMsg_t; + + +#endif + + From 3d1577e3ab2df8cb0dab5abd63bb68894deeb8c2 Mon Sep 17 00:00:00 2001 From: Mathew Benson Date: Mon, 27 Jun 2022 15:15:04 -0500 Subject: [PATCH 2/4] Added in the ECL controllers, but still need to merge in FPC updates to clear build errors. --- apps/fac/fsw/for_build/CMakeLists.txt | 2 +- apps/fac/fsw/src/ECL_Controller.cpp | 83 ++++++++++- apps/fac/fsw/src/ECL_Controller.hpp | 65 ++++++++- apps/fac/fsw/src/ECL_PitchController.cpp | 112 ++++++++++++++- apps/fac/fsw/src/ECL_PitchController.hpp | 31 +++- apps/fac/fsw/src/ECL_RollController.cpp | 92 +++++++++++- apps/fac/fsw/src/ECL_RollController.hpp | 11 +- apps/fac/fsw/src/ECL_WheelController.cpp | 89 +++++++++++- apps/fac/fsw/src/ECL_WheelController.hpp | 13 +- apps/fac/fsw/src/ECL_YawController.cpp | 176 ++++++++++++++++++++++- apps/fac/fsw/src/ECL_YawController.hpp | 37 ++++- 11 files changed, 686 insertions(+), 25 deletions(-) diff --git a/apps/fac/fsw/for_build/CMakeLists.txt b/apps/fac/fsw/for_build/CMakeLists.txt index 78c1041bf..b7498198d 100644 --- a/apps/fac/fsw/for_build/CMakeLists.txt +++ b/apps/fac/fsw/for_build/CMakeLists.txt @@ -51,7 +51,7 @@ buildliner_add_app_def(fac ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_WheelController.cpp ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_WheelController.hpp ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_YawController.cpp - ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_YawController.cpp + ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/ECL_YawController.hpp ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_events.h ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_msg.h ${PROJECT_SOURCE_DIR}/apps/fac/fsw/src/fac_private_ids.h diff --git a/apps/fac/fsw/src/ECL_Controller.cpp b/apps/fac/fsw/src/ECL_Controller.cpp index d5af3978e..05e0a84a6 100644 --- a/apps/fac/fsw/src/ECL_Controller.cpp +++ b/apps/fac/fsw/src/ECL_Controller.cpp @@ -34,12 +34,91 @@ #include "ECL_Controller.hpp" -ECL_Controller::ECL_Controller() +ECL_Controller::ECL_Controller(const char *name) { + init(); +} + + +void ECL_Controller::init(void) +{ + _last_run = 0; + _tc = 0.1f; + _k_p = 0.0f; + _k_i = 0.0f; + _k_ff = 0.0f; + _integrator_max = 0.0f; + _max_rate = 0.0f; + _last_output = 0.0f; + _integrator = 0.0f; + _rate_error = 0.0f; + _rate_setpoint = 0.0f; + _bodyrate_setpoint = 0.0f; +} + +void ECL_Controller::reset_integrator() +{ + _integrator = 0.0f; +} + +void ECL_Controller::set_time_constant(float time_constant) +{ + if (time_constant > 0.1f && time_constant < 3.0f) { + _tc = time_constant; + } +} + +void ECL_Controller::set_k_p(float k_p) +{ + _k_p = k_p; +} +void ECL_Controller::set_k_i(float k_i) +{ + _k_i = k_i; +} + +void ECL_Controller::set_k_ff(float k_ff) +{ + _k_ff = k_ff; } -ECL_Controller::~ECL_Controller() +void ECL_Controller::set_integrator_max(float max) { + _integrator_max = max; +} + +void ECL_Controller::set_max_rate(float max_rate) +{ + _max_rate = max_rate; +} + +float ECL_Controller::get_rate_error() +{ + return _rate_error; +} + +float ECL_Controller::get_desired_rate() +{ + return _rate_setpoint; +} + +float ECL_Controller::get_desired_bodyrate() +{ + return _bodyrate_setpoint; +} + +float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) +{ + float airspeed_result = airspeed; + + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed_result = 0.5f * (minspeed + maxspeed); + + } else if (airspeed < minspeed) { + airspeed_result = minspeed; + } + return airspeed_result; } diff --git a/apps/fac/fsw/src/ECL_Controller.hpp b/apps/fac/fsw/src/ECL_Controller.hpp index 40a9b5d72..778cae86b 100644 --- a/apps/fac/fsw/src/ECL_Controller.hpp +++ b/apps/fac/fsw/src/ECL_Controller.hpp @@ -34,12 +34,73 @@ #ifndef ECL_CONTROLLER_HPP #define ECL_CONTROLLER_HPP +#include +#include "cfe.h" + +struct ECL_ControlData { + float roll; + float pitch; + float yaw; + float body_x_rate; + float body_y_rate; + float body_z_rate; + float roll_setpoint; + float pitch_setpoint; + float yaw_setpoint; + float roll_rate_setpoint; + float pitch_rate_setpoint; + float yaw_rate_setpoint; + float airspeed_min; + float airspeed_max; + float airspeed; + float scaler; + float groundspeed; + float groundspeed_scaler; + bool lock_integrator; +}; class ECL_Controller { public: - ECL_Controller(); - ~ECL_Controller(); + ECL_Controller(const char *name); + ~ECL_Controller() = default; + + virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0; + virtual float control_euler_rate(const struct ECL_ControlData &ctl_data) = 0; + virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0; + + /* Setters */ + void set_time_constant(float time_constant); + void set_k_p(float k_p); + void set_k_i(float k_i); + void set_k_ff(float k_ff); + void set_integrator_max(float max); + void set_max_rate(float max_rate); + void set_bodyrate_setpoint(float rate) {_bodyrate_setpoint = rate;} + + /* Getters */ + float get_rate_error(); + float get_desired_rate(); + float get_desired_bodyrate(); + + void reset_integrator(); + + void init(void); + +protected: + uint64 _last_run; + float _tc; + float _k_p; + float _k_i; + float _k_ff; + float _integrator_max; + float _max_rate; + float _last_output; + float _integrator; + float _rate_error; + float _rate_setpoint; + float _bodyrate_setpoint; + float constrain_airspeed(float airspeed, float minspeed, float maxspeed); }; #endif diff --git a/apps/fac/fsw/src/ECL_PitchController.cpp b/apps/fac/fsw/src/ECL_PitchController.cpp index 428ef9520..cbd098318 100644 --- a/apps/fac/fsw/src/ECL_PitchController.cpp +++ b/apps/fac/fsw/src/ECL_PitchController.cpp @@ -33,13 +33,121 @@ #include "ECL_PitchController.hpp" +#include "math/Limits.hpp" +#include "math/Vector2F.hpp" +#include "math/Euler.hpp" +#include "math/Quaternion.hpp" +#include "px4lib.h" -ECL_PitchController::ECL_PitchController() + + +ECL_PitchController::ECL_PitchController() : + ECL_Controller("pitch"), + _max_rate_neg(0.0f), + _roll_ff(0.0f) +{ + +} + +float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(ctl_data.pitch_setpoint) && + isfinite(ctl_data.roll) && + isfinite(ctl_data.pitch) && + isfinite(ctl_data.airspeed))) + { + /* TODO: Replace with CFE EVS Event message. */ + //warnx("not controlling pitch"); + return _rate_setpoint; + } + + /* Calculate the error */ + float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = pitch_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f && _max_rate_neg > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint; + } + + } + + return _rate_setpoint; +} + +float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(ctl_data.roll) && + isfinite(ctl_data.pitch) && + isfinite(ctl_data.body_y_rate) && + isfinite(ctl_data.body_z_rate) && + isfinite(ctl_data.yaw_rate_setpoint) && + isfinite(ctl_data.airspeed_min) && + isfinite(ctl_data.airspeed_max) && + isfinite(ctl_data.scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64 dt_micros = PX4LIB_GetPX4ElapsedTimeUs(_last_run); + _last_run = PX4LIB_GetPX4TimeUs(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + _rate_error = _bodyrate_setpoint - ctl_data.body_y_rate; + + if (!lock_integrator && _k_i > 0.0f) { + float id = _rate_error * dt * ctl_data.scaler; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id * _k_i; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + + _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + + integrator_constrained; //scaler is proportional to 1/airspeed + + return math::constrain(_last_output, -1.0f, 1.0f); } -ECL_PitchController::~ECL_PitchController() +float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data) { + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; + return control_bodyrate(ctl_data); } + diff --git a/apps/fac/fsw/src/ECL_PitchController.hpp b/apps/fac/fsw/src/ECL_PitchController.hpp index 911aa4a77..445272872 100644 --- a/apps/fac/fsw/src/ECL_PitchController.hpp +++ b/apps/fac/fsw/src/ECL_PitchController.hpp @@ -36,11 +36,36 @@ #include "ECL_Controller.hpp" -class ECL_PitchController : public ECL_Controller +class ECL_PitchController : + public ECL_Controller { public: - ECL_PitchController(); - ~ECL_PitchController(); + ECL_PitchController(); + ~ECL_PitchController() = default; + + float control_attitude(const struct ECL_ControlData &ctl_data); + float control_euler_rate(const struct ECL_ControlData &ctl_data); + float control_bodyrate(const struct ECL_ControlData &ctl_data); + + /* Additional Setters */ + void set_max_rate_pos(float max_rate_pos) + { + _max_rate = max_rate_pos; + } + + void set_max_rate_neg(float max_rate_neg) + { + _max_rate_neg = max_rate_neg; + } + + void set_roll_ff(float roll_ff) + { + _roll_ff = roll_ff; + } + + protected: + float _max_rate_neg; + float _roll_ff; }; #endif diff --git a/apps/fac/fsw/src/ECL_RollController.cpp b/apps/fac/fsw/src/ECL_RollController.cpp index c9fdf8aa5..fbd68fc40 100644 --- a/apps/fac/fsw/src/ECL_RollController.cpp +++ b/apps/fac/fsw/src/ECL_RollController.cpp @@ -33,13 +33,101 @@ #include "ECL_RollController.hpp" +#include "math/Limits.hpp" +#include "px4lib.h" -ECL_RollController::ECL_RollController() +ECL_RollController::ECL_RollController() : + ECL_Controller("roll") { +} + +float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(ctl_data.roll_setpoint) && isfinite(ctl_data.roll))) { + return _rate_setpoint; + } + + /* Calculate error */ + float roll_error = ctl_data.roll_setpoint - ctl_data.roll; + + /* Apply P controller */ + _rate_setpoint = roll_error / _tc; + + /* limit the rate */ //XXX: move to body angluar rates + if (_max_rate > 0.01f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + return _rate_setpoint; } -ECL_RollController::~ECL_RollController() +float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(ctl_data.pitch) && + isfinite(ctl_data.body_x_rate) && + isfinite(ctl_data.body_z_rate) && + isfinite(ctl_data.yaw_rate_setpoint) && + isfinite(ctl_data.airspeed_min) && + isfinite(ctl_data.airspeed_max) && + isfinite(ctl_data.scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64 dt_micros = PX4LIB_GetPX4ElapsedTimeUs(_last_run); + _last_run = PX4LIB_GetPX4TimeUs(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - ctl_data.body_x_rate; //body angular rate error + + if (!lock_integrator && _k_i > 0.0f) { + + float id = _rate_error * dt * ctl_data.scaler; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id * _k_i; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + + _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + + integrator_constrained; //scaler is proportional to 1/airspeed + + return math::constrain(_last_output, -1.0f, 1.0f); +} + +float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_data) +{ + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; + + return control_bodyrate(ctl_data); } diff --git a/apps/fac/fsw/src/ECL_RollController.hpp b/apps/fac/fsw/src/ECL_RollController.hpp index 72bb38a3f..6f51a4c12 100644 --- a/apps/fac/fsw/src/ECL_RollController.hpp +++ b/apps/fac/fsw/src/ECL_RollController.hpp @@ -36,11 +36,16 @@ #include "ECL_Controller.hpp" -class ECL_RollController : public ECL_Controller +class ECL_RollController : + public ECL_Controller { public: - ECL_RollController(); - ~ECL_RollController(); + ECL_RollController(); + ~ECL_RollController() = default; + + float control_attitude(const struct ECL_ControlData &ctl_data); + float control_euler_rate(const struct ECL_ControlData &ctl_data); + float control_bodyrate(const struct ECL_ControlData &ctl_data); }; #endif diff --git a/apps/fac/fsw/src/ECL_WheelController.cpp b/apps/fac/fsw/src/ECL_WheelController.cpp index 0586b1ae8..5d3ff03d1 100644 --- a/apps/fac/fsw/src/ECL_WheelController.cpp +++ b/apps/fac/fsw/src/ECL_WheelController.cpp @@ -33,13 +33,98 @@ #include "ECL_WheelController.hpp" +#include "math/Limits.hpp" +#include "px4lib.h" +#include "geo/geo.h" -ECL_WheelController::ECL_WheelController() + +ECL_WheelController::ECL_WheelController() : + ECL_Controller("wheel") +{ +} + +float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(ctl_data.body_z_rate) && + isfinite(ctl_data.groundspeed) && + isfinite(ctl_data.groundspeed_scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64 dt_micros = PX4LIB_GetPX4ElapsedTimeUs(_last_run); + _last_run = PX4LIB_GetPX4TimeUs(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + /* input conditioning */ + float min_speed = 1.0f; + + /* Calculate body angular rate error */ + _rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error + + if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { + + float id = _rate_error * dt * ctl_data.groundspeed_scaler; + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id * _k_i; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + + ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + integrator_constrained); + + return math::constrain(_last_output, -1.0f, 1.0f); } -ECL_WheelController::~ECL_WheelController() + +float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(ctl_data.yaw_setpoint) && + isfinite(ctl_data.yaw))) { + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = _wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw); + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + return _rate_setpoint; } diff --git a/apps/fac/fsw/src/ECL_WheelController.hpp b/apps/fac/fsw/src/ECL_WheelController.hpp index 74aabb25e..f72f92571 100644 --- a/apps/fac/fsw/src/ECL_WheelController.hpp +++ b/apps/fac/fsw/src/ECL_WheelController.hpp @@ -36,11 +36,18 @@ #include "ECL_Controller.hpp" -class ECL_WheelController : public ECL_Controller +class ECL_WheelController : + public ECL_Controller { public: - ECL_WheelController(); - ~ECL_WheelController(); + ECL_WheelController(); + ~ECL_WheelController() = default; + + float control_attitude(const struct ECL_ControlData &ctl_data); + + float control_bodyrate(const struct ECL_ControlData &ctl_data); + + float control_euler_rate(const struct ECL_ControlData &ctl_data) {return 0;} }; #endif diff --git a/apps/fac/fsw/src/ECL_YawController.cpp b/apps/fac/fsw/src/ECL_YawController.cpp index 729b517f3..d1673314a 100644 --- a/apps/fac/fsw/src/ECL_YawController.cpp +++ b/apps/fac/fsw/src/ECL_YawController.cpp @@ -33,13 +33,185 @@ #include "ECL_YawController.hpp" +#include "math/Limits.hpp" +#include "px4lib.h" -ECL_YawController::ECL_YawController() + + + +ECL_YawController::ECL_YawController() : + ECL_Controller("yaw"), + _coordinated_min_speed(1.0f), + _max_rate(0.0f), /* disable by default */ + _coordinated_method(0) +{ +} + +float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + switch (_coordinated_method) { + case COORD_METHOD_OPEN: + return control_attitude_impl_openloop(ctl_data); + + case COORD_METHOD_CLOSEACC: + return control_attitude_impl_accclosedloop(ctl_data); + + default: + static uint64 last_print = 0; + + if (PX4LIB_GetPX4ElapsedTimeUs(last_print) > 5e6) { + /* TODO: Replace with CFE EVS event message */ + //warnx("invalid param setting FW_YCO_METHOD"); + last_print = PX4LIB_GetPX4TimeUs(); + } + } + + return _rate_setpoint; +} + +float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(ctl_data.roll) && + isfinite(ctl_data.pitch) && + isfinite(ctl_data.roll_rate_setpoint) && + isfinite(ctl_data.pitch_rate_setpoint))) { + return _rate_setpoint; + } + + float constrained_roll; + bool inverted = false; + + /* roll is used as feedforward term and inverted flight needs to be considered */ + if (fabsf(ctl_data.roll) < math::radians(90.0f)) { + /* not inverted, but numerically still potentially close to infinity */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f)); + + } else { + inverted = true; + + // inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity + //note: the ranges are extended by 10 deg here to avoid numeric resolution effects + if (ctl_data.roll > 0.0f) { + /* right hemisphere */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); + + } else { + /* left hemisphere */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(-180.0f), math::radians(-100.0f)); + } + } + + constrained_roll = math::constrain(constrained_roll, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint)); + + + if (!inverted) { + /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ + _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * 9.81f / (ctl_data.airspeed < ctl_data.airspeed_min ? + ctl_data.airspeed_min : ctl_data.airspeed); + } + + /* limit the rate */ //XXX: move to body angluar rates + + if (_max_rate > 0.01f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + if (!isfinite(_rate_setpoint)) { + /* TODO: Replace with CFE EVS event message. */ + //warnx("yaw rate sepoint not finite"); + _rate_setpoint = 0.0f; + } + + return _rate_setpoint; +} + +float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.body_y_rate) && + isfinite(ctl_data.body_z_rate) && isfinite(ctl_data.pitch_rate_setpoint) && + isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) && + isfinite(ctl_data.scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64 dt_micros = PX4LIB_GetPX4ElapsedTimeUs(_last_run); + _last_run = PX4LIB_GetPX4TimeUs(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + /* input conditioning */ + float airspeed = ctl_data.airspeed; + + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); + + } else if (airspeed < ctl_data.airspeed_min) { + airspeed = ctl_data.airspeed_min; + } + + /* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */ + if (_coordinated_method == COORD_METHOD_CLOSEACC) { + // XXX lateral acceleration needs to go into integrator with a gain + //_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch))); + } + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; // body angular rate error + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + + float id = _rate_error * dt; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id * _k_i; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * + ctl_data.scaler; //scaler is proportional to 1/airspeed + + + return math::constrain(_last_output, -1.0f, 1.0f); +} + +float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data) +{ + /* dont set a rate setpoint */ + return 0.0f; } -ECL_YawController::~ECL_YawController() +float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data) { + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + + cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; + + return control_bodyrate(ctl_data); } diff --git a/apps/fac/fsw/src/ECL_YawController.hpp b/apps/fac/fsw/src/ECL_YawController.hpp index 95516bd5d..30ecd92de 100644 --- a/apps/fac/fsw/src/ECL_YawController.hpp +++ b/apps/fac/fsw/src/ECL_YawController.hpp @@ -36,11 +36,42 @@ #include "ECL_Controller.hpp" -class ECL_YawController : public ECL_Controller +class ECL_YawController : + public ECL_Controller { public: - ECL_YawController(); - ~ECL_YawController(); + ECL_YawController(); + ~ECL_YawController() = default; + + float control_attitude(const struct ECL_ControlData &ctl_data); + float control_euler_rate(const struct ECL_ControlData &ctl_data); + float control_bodyrate(const struct ECL_ControlData &ctl_data); + + /* Additional setters */ + void set_coordinated_min_speed(float coordinated_min_speed) + { + _coordinated_min_speed = coordinated_min_speed; + } + + void set_coordinated_method(int32_t coordinated_method) + { + _coordinated_method = coordinated_method; + } + + enum { + COORD_METHOD_OPEN = 0, + COORD_METHOD_CLOSEACC = 1 + }; + +protected: + float _coordinated_min_speed; + float _max_rate; + + int32_t _coordinated_method; + + float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data); + + float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); }; #endif From adc698b3391a2a21e0d595e2d7213c4b6226288e Mon Sep 17 00:00:00 2001 From: Young Nielson Date: Tue, 28 Jun 2022 09:01:06 -0500 Subject: [PATCH 3/4] Add CurrentValueTable(CVT) and PX4 Message handling routine --- apps/fac/fsw/platform_inc/fac_platform_cfg.h | 4 +- apps/fac/fsw/src/fac_app.cpp | 3 + apps/fac/fsw/src/fac_app.hpp | 13 ++ apps/fac/fsw/src/fac_cmds_utils.cpp | 2 + apps/fac/fsw/src/fac_data_utils.cpp | 132 ++++++++---------- apps/fac/fsw/src/fac_data_utils.hpp | 11 -- .../fixedwing/cpd/sitl/target/CMakeLists.txt | 9 -- 7 files changed, 82 insertions(+), 92 deletions(-) diff --git a/apps/fac/fsw/platform_inc/fac_platform_cfg.h b/apps/fac/fsw/platform_inc/fac_platform_cfg.h index 7a825a130..813c4fcce 100644 --- a/apps/fac/fsw/platform_inc/fac_platform_cfg.h +++ b/apps/fac/fsw/platform_inc/fac_platform_cfg.h @@ -92,7 +92,7 @@ ** \par Limits: ** minimum of 1, max of CFE_SB_MAX_PIPE_DEPTH(256). */ -#define FAC_DATA_PIPE_DEPTH (5) +#define FAC_DATA_PIPE_DEPTH (4) /** \brief Pipe name for the Scheduler pipe ** @@ -110,7 +110,7 @@ ** application if no messages arrive on this pipe. */ #define FAC_DATA_PIPE_PEND_TIME (2000) -#define FAC_DATA_PIPE_RESERVED (FAC_DATA_PIPE_DEPTH - 1) +#define FAC_DATA_PIPE_RESERVED (1) /** \brief The config table default filename ** diff --git a/apps/fac/fsw/src/fac_app.cpp b/apps/fac/fsw/src/fac_app.cpp index 78f34e8ab..bcaf2b80d 100644 --- a/apps/fac/fsw/src/fac_app.cpp +++ b/apps/fac/fsw/src/fac_app.cpp @@ -35,6 +35,8 @@ #include #include +#include + #include "fac_tbldefs.h" #include "fac_app.hpp" #include "fac_fac.hpp" @@ -46,6 +48,7 @@ #include FAC_AppData_t FAC_AppData; +FAC_CurrentValueTable_t CVT; FixedwingAttitudeControl objFAC; AppCommandProcess objCmds; diff --git a/apps/fac/fsw/src/fac_app.hpp b/apps/fac/fsw/src/fac_app.hpp index 8c3a8ef7a..e8417c536 100644 --- a/apps/fac/fsw/src/fac_app.hpp +++ b/apps/fac/fsw/src/fac_app.hpp @@ -61,6 +61,19 @@ typedef struct FAC_ConfigTbl_t *ConfigTblPtr; } FAC_AppData_t; +typedef struct +{ + PX4_AirspeedMsg_t Airspeed; // check : should be vector + PX4_BatteryStatusMsg_t BatteryStatus; + PX4_ManualControlSetpointMsg_t ManualControlSetpoint; + PX4_VehicleAttitudeMsg_t VehicleAttitude; + PX4_VehicleAttitudeSetpointMsg_t VehicleAttitudeSetpoint; + PX4_VehicleControlModeMsg_t VehicleControlMode; + PX4_VehicleGlobalPositionMsg_t VehicleGlobalPosition; + PX4_VehicleLandDetectedMsg_t VehicleLandDetected; + PX4_VehicleStatusMsg_t VehicleStatus; +} FAC_CurrentValueTable_t; + /************************************************************************/ /** \brief CFS Fixedwing Attitude Control App Task (FAC) application diff --git a/apps/fac/fsw/src/fac_cmds_utils.cpp b/apps/fac/fsw/src/fac_cmds_utils.cpp index 034e6490f..4b71a824f 100644 --- a/apps/fac/fsw/src/fac_cmds_utils.cpp +++ b/apps/fac/fsw/src/fac_cmds_utils.cpp @@ -34,6 +34,8 @@ #include #include +#include + #include "fac_tbldefs.h" #include "fac_app.hpp" #include "fac_cmds_utils.hpp" diff --git a/apps/fac/fsw/src/fac_data_utils.cpp b/apps/fac/fsw/src/fac_data_utils.cpp index d2070dc8a..ce5f7932f 100644 --- a/apps/fac/fsw/src/fac_data_utils.cpp +++ b/apps/fac/fsw/src/fac_data_utils.cpp @@ -46,6 +46,8 @@ extern FAC_AppData_t FAC_AppData; +extern FAC_CurrentValueTable_t CVT; + AppDataProcess::AppDataProcess() { @@ -204,56 +206,6 @@ int32 AppDataProcess::VerifyDataMsgLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpecte return (iStatus); } -void AppDataProcess::HandleAirspeed() -{ - return; -} - -void AppDataProcess::HandleVehAttitude() -{ - return; -} - -void AppDataProcess::HandleVehAttitudeSetPoint() -{ - return; -} - -void AppDataProcess::HandleVehControlMode() -{ - return; -} - -void AppDataProcess::HandleParameterUpdate() -{ - return; -} - -void AppDataProcess::HandleManualControlSetpoint() -{ - return; -} - -void AppDataProcess::HandleVehGlobalPosition() -{ - return; -} - -void AppDataProcess::HandleVehStatus() -{ - return; -} - -void AppDataProcess::HandleVehLandDetected() -{ - return; -} - -void AppDataProcess::HandleBatteryStatus() -{ - return; -} - int32 AppDataProcess::RcvDataMsg(int32 iBlocking) { int32 iStatus = CFE_SUCCESS; @@ -272,43 +224,55 @@ int32 AppDataProcess::RcvDataMsg(int32 iBlocking) switch (DataMsgId) { case PX4_AIRSPEED_MID: - HandleAirspeed(); + CFE_PSP_MemCpy(&CVT.Airspeed, DataMsgPtr, + sizeof(CVT.Airspeed)); // check vector break; - case PX4_VEHICLE_ATTITUDE_MID: - HandleVehAttitude(); - break; - - case PX4_VEHICLE_ATTITUDE_SETPOINT_MID: - HandleVehAttitudeSetPoint(); + case PX4_BATTERY_STATUS_MID: + CFE_PSP_MemCpy(&CVT.BatteryStatus, DataMsgPtr, + sizeof(CVT.BatteryStatus)); // check vector break; - case PX4_VEHICLE_CONTROL_MODE_MID: - HandleVehControlMode(); + case PX4_MANUAL_CONTROL_SETPOINT_MID: + CFE_PSP_MemCpy(&CVT.ManualControlSetpoint, DataMsgPtr, + sizeof(CVT.ManualControlSetpoint)); break; +#if 0 // check case PX4_PARAMETER_UPDATE_MID: HandleParameterUpdate(); break; +#endif - case PX4_MANUAL_CONTROL_SETPOINT_MID: - HandleManualControlSetpoint(); + case PX4_VEHICLE_ATTITUDE_MID: + CFE_PSP_MemCpy(&CVT.VehicleAttitude, DataMsgPtr, + sizeof(CVT.VehicleAttitude)); break; - case PX4_VEHICLE_GLOBAL_POSITION_MID: - HandleVehGlobalPosition(); + case PX4_VEHICLE_ATTITUDE_SETPOINT_MID: + CFE_PSP_MemCpy(&CVT.VehicleAttitudeSetpoint, DataMsgPtr, + sizeof(CVT.VehicleAttitudeSetpoint)); break; - case PX4_VEHICLE_STATUS_MID: - HandleVehStatus(); + case PX4_VEHICLE_CONTROL_MODE_MID: + CFE_PSP_MemCpy(&CVT.VehicleControlMode, DataMsgPtr, + sizeof(CVT.VehicleControlMode)); + break; + + + case PX4_VEHICLE_GLOBAL_POSITION_MID: + CFE_PSP_MemCpy(&CVT.VehicleGlobalPosition, DataMsgPtr, + sizeof(CVT.VehicleGlobalPosition)); break; case PX4_VEHICLE_LAND_DETECTED_MID: - HandleVehLandDetected(); + CFE_PSP_MemCpy(&CVT.VehicleLandDetected, DataMsgPtr, + sizeof(CVT.VehicleLandDetected)); break; - case PX4_BATTERY_STATUS_MID: - HandleBatteryStatus(); + case PX4_VEHICLE_STATUS_MID: + CFE_PSP_MemCpy(&CVT.VehicleStatus, DataMsgPtr, + sizeof(CVT.VehicleStatus)); break; default: @@ -355,7 +319,7 @@ int32 AppDataProcess::Subscribe(CFE_SB_MsgId_t MsgId) { int32 iStatus = CFE_SUCCESS; - iStatus = CFE_SB_Subscribe(MsgId, DataPipeId); + iStatus = CFE_SB_SubscribeEx(MsgId, DataPipeId, CFE_SB_Default_Qos, FAC_DATA_PIPE_RESERVED); if (iStatus != CFE_SUCCESS) { CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, @@ -465,6 +429,24 @@ int32 AppDataProcess::InitData() // check cfe function for length CFE_SB_InitMsg(&OutData, FAC_OUT_DATA_MID, sizeof(OutData), TRUE); // check length + CFE_SB_InitMsg(&CVT.Airspeed, PX4_AIRSPEED_MID, sizeof(CVT.Airspeed), TRUE); + CFE_SB_InitMsg(&CVT.BatteryStatus, PX4_BATTERY_STATUS_MID, + sizeof(CVT.BatteryStatus), TRUE); + CFE_SB_InitMsg(&CVT.ManualControlSetpoint, PX4_MANUAL_CONTROL_SETPOINT_MID, + sizeof(CVT.ManualControlSetpoint), TRUE); + CFE_SB_InitMsg(&CVT.VehicleAttitude, PX4_VEHICLE_ATTITUDE_MID, + sizeof(CVT.VehicleAttitude), TRUE); + CFE_SB_InitMsg(&CVT.VehicleAttitudeSetpoint, PX4_VEHICLE_ATTITUDE_SETPOINT_MID, + sizeof(CVT.VehicleAttitudeSetpoint), TRUE); + CFE_SB_InitMsg(&CVT.VehicleControlMode, PX4_VEHICLE_CONTROL_MODE_MID, + sizeof(CVT.VehicleControlMode), TRUE); + CFE_SB_InitMsg(&CVT.VehicleGlobalPosition, PX4_VEHICLE_GLOBAL_POSITION_MID, + sizeof(CVT.VehicleGlobalPosition), TRUE); + CFE_SB_InitMsg(&CVT.VehicleLandDetected, PX4_VEHICLE_LAND_DETECTED_MID, + sizeof(CVT.VehicleLandDetected), TRUE); + CFE_SB_InitMsg(&CVT.VehicleStatus, PX4_VEHICLE_STATUS_MID, + sizeof(CVT.VehicleStatus), TRUE); + return (iStatus); } @@ -472,7 +454,7 @@ int32 AppDataProcess::InitDataPipe() { int32 iStatus = CFE_SUCCESS; - iStatus = CFE_SB_CreatePipe(&DataPipeId, FAC_DATA_PIPE_DEPTH, FAC_DATA_PIPE_NAME); + iStatus = CFE_SB_CreatePipe(&DataPipeId, FAC_DATA_PIPE_DEPTH, FAC_DATA_PIPE_NAME); // check depth if (iStatus == CFE_SUCCESS) { iStatus = Subscribe(PX4_AIRSPEED_MID); @@ -480,46 +462,56 @@ int32 AppDataProcess::InitDataPipe() { goto InitDataPipe_Exit_Tag; } + iStatus = Subscribe(PX4_VEHICLE_ATTITUDE_MID); if (iStatus != CFE_SUCCESS) { goto InitDataPipe_Exit_Tag; } + iStatus = Subscribe(PX4_VEHICLE_ATTITUDE_SETPOINT_MID); if (iStatus != CFE_SUCCESS) { goto InitDataPipe_Exit_Tag; } + iStatus = Subscribe(PX4_VEHICLE_CONTROL_MODE_MID); if (iStatus != CFE_SUCCESS) { goto InitDataPipe_Exit_Tag; } +#if 0 iStatus = Subscribe(PX4_PARAMETER_UPDATE_MID); if (iStatus != CFE_SUCCESS) { goto InitDataPipe_Exit_Tag; } +#endif + iStatus = Subscribe(PX4_MANUAL_CONTROL_SETPOINT_MID); if (iStatus != CFE_SUCCESS) { goto InitDataPipe_Exit_Tag; } + iStatus = Subscribe(PX4_VEHICLE_GLOBAL_POSITION_MID); if (iStatus != CFE_SUCCESS) { goto InitDataPipe_Exit_Tag; } + iStatus = Subscribe(PX4_VEHICLE_STATUS_MID); if (iStatus != CFE_SUCCESS) { goto InitDataPipe_Exit_Tag; } + iStatus = Subscribe(PX4_VEHICLE_LAND_DETECTED_MID); if (iStatus != CFE_SUCCESS) { goto InitDataPipe_Exit_Tag; } + iStatus = Subscribe(PX4_BATTERY_STATUS_MID); if (iStatus != CFE_SUCCESS) { diff --git a/apps/fac/fsw/src/fac_data_utils.hpp b/apps/fac/fsw/src/fac_data_utils.hpp index ba906d9fc..4e2791640 100644 --- a/apps/fac/fsw/src/fac_data_utils.hpp +++ b/apps/fac/fsw/src/fac_data_utils.hpp @@ -70,17 +70,6 @@ class AppDataProcess int32 Subscribe(CFE_SB_MsgId_t MsgId); int32 VerifyDataMsgLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen); - void HandleAirspeed(); - void HandleVehAttitude(); - void HandleVehAttitudeSetPoint(); - void HandleVehControlMode(); - void HandleParameterUpdate(); - void HandleManualControlSetpoint(); - void HandleVehGlobalPosition(); - void HandleVehStatus(); - void HandleVehLandDetected(); - void HandleBatteryStatus(); - int32 AcquireConfigPointers(); void ProcessNewConfigTbl(); diff --git a/config/obc/fixedwing/cpd/sitl/target/CMakeLists.txt b/config/obc/fixedwing/cpd/sitl/target/CMakeLists.txt index fa86c6397..37f11d73f 100644 --- a/config/obc/fixedwing/cpd/sitl/target/CMakeLists.txt +++ b/config/obc/fixedwing/cpd/sitl/target/CMakeLists.txt @@ -137,13 +137,4 @@ buildliner_add_app( EMBEDDED DEFINITION ${PROJECT_SOURCE_DIR}/apps/fac/fsw/for_build CONFIG ${PROJECT_SOURCE_DIR}/config/shared/apps/fac - CONFIG_SOURCES - ${PROJECT_SOURCE_DIR}/config/shared/apps/fac/tables/fac_config.c -) - -buildliner_add_app( - aspd4525 - EMBEDDED - DEFINITION ${PROJECT_SOURCE_DIR}/apps/aspd4525/fsw/for_build - CONFIG ${PROJECT_SOURCE_DIR}/config/obc/fixedwing/cpd/apps/aspd4525 ) From da9282649cd62434bd8698a16a2aa8fd1fd52507 Mon Sep 17 00:00:00 2001 From: Young Nielson Date: Tue, 28 Jun 2022 15:58:00 -0500 Subject: [PATCH 4/4] Added debugging messages --- apps/fac/fsw/src/fac_data_utils.cpp | 29 +++++++++++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/apps/fac/fsw/src/fac_data_utils.cpp b/apps/fac/fsw/src/fac_data_utils.cpp index ce5f7932f..057e11115 100644 --- a/apps/fac/fsw/src/fac_data_utils.cpp +++ b/apps/fac/fsw/src/fac_data_utils.cpp @@ -59,7 +59,6 @@ AppDataProcess::~AppDataProcess() void AppDataProcess::SendOutData() { -#if 0 // check this /* TODO: Add code to update output data, if needed, here. */ CFE_SB_TimeStampMsg((CFE_SB_Msg_t*)&OutData); @@ -68,7 +67,6 @@ void AppDataProcess::SendOutData() { /* TODO: Decide what to do if the send message fails. */ } -#endif } void AppDataProcess::ProcessNewConfigTbl() @@ -224,16 +222,25 @@ int32 AppDataProcess::RcvDataMsg(int32 iBlocking) switch (DataMsgId) { case PX4_AIRSPEED_MID: + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "RcvDataMsg:Rcvd PX4_AIRSPEED_MID 0x%04X (0x%08X)", + (unsigned short)DataMsgId, (unsigned int)iStatus); CFE_PSP_MemCpy(&CVT.Airspeed, DataMsgPtr, sizeof(CVT.Airspeed)); // check vector break; case PX4_BATTERY_STATUS_MID: + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "RcvDataMsg:Rcvd PX4_BATTERY_STATUS_MID 0x%04X (0x%08X)", + (unsigned short)DataMsgId, (unsigned int)iStatus); CFE_PSP_MemCpy(&CVT.BatteryStatus, DataMsgPtr, sizeof(CVT.BatteryStatus)); // check vector break; case PX4_MANUAL_CONTROL_SETPOINT_MID: + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "RcvDataMsg:Rcvd PX4_MANUAL_CONTROL_SETPOINT_MID 0x%04X (0x%08X)", + (unsigned short)DataMsgId, (unsigned int)iStatus); CFE_PSP_MemCpy(&CVT.ManualControlSetpoint, DataMsgPtr, sizeof(CVT.ManualControlSetpoint)); break; @@ -245,32 +252,50 @@ int32 AppDataProcess::RcvDataMsg(int32 iBlocking) #endif case PX4_VEHICLE_ATTITUDE_MID: + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "RcvDataMsg:Rcvd PX4_VEHICLE_ATTITUDE_MID 0x%04X (0x%08X)", + (unsigned short)DataMsgId, (unsigned int)iStatus); CFE_PSP_MemCpy(&CVT.VehicleAttitude, DataMsgPtr, sizeof(CVT.VehicleAttitude)); break; case PX4_VEHICLE_ATTITUDE_SETPOINT_MID: + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "RcvDataMsg:Rcvd PX4_VEHICLE_ATTITUDE_SETPOINT_MID 0x%04X (0x%08X)", + (unsigned short)DataMsgId, (unsigned int)iStatus); CFE_PSP_MemCpy(&CVT.VehicleAttitudeSetpoint, DataMsgPtr, sizeof(CVT.VehicleAttitudeSetpoint)); break; case PX4_VEHICLE_CONTROL_MODE_MID: + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "RcvDataMsg:Rcvd PX4_VEHICLE_CONTROL_MODE_MID 0x%04X (0x%08X)", + (unsigned short)DataMsgId, (unsigned int)iStatus); CFE_PSP_MemCpy(&CVT.VehicleControlMode, DataMsgPtr, sizeof(CVT.VehicleControlMode)); break; case PX4_VEHICLE_GLOBAL_POSITION_MID: + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "RcvDataMsg:Rcvd PX4_VEHICLE_GLOBAL_POSITION_MID 0x%04X (0x%08X)", + (unsigned short)DataMsgId, (unsigned int)iStatus); CFE_PSP_MemCpy(&CVT.VehicleGlobalPosition, DataMsgPtr, sizeof(CVT.VehicleGlobalPosition)); break; case PX4_VEHICLE_LAND_DETECTED_MID: + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "RcvDataMsg:Rcvd PX4_VEHICLE_LAND_DETECTED_MID 0x%04X (0x%08X)", + (unsigned short)DataMsgId, (unsigned int)iStatus); CFE_PSP_MemCpy(&CVT.VehicleLandDetected, DataMsgPtr, sizeof(CVT.VehicleLandDetected)); break; case PX4_VEHICLE_STATUS_MID: + CFE_EVS_SendEvent(FAC_INF_EID, CFE_EVS_INFORMATION, + "RcvDataMsg:Rcvd PX4_VEHICLE_STATUS_MID 0x%04X (0x%08X)", + (unsigned short)DataMsgId, (unsigned int)iStatus); CFE_PSP_MemCpy(&CVT.VehicleStatus, DataMsgPtr, sizeof(CVT.VehicleStatus)); break;