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/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_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..b9a5d0752 100644 --- a/apps/fac/fsw/src/fac_app.cpp +++ b/apps/fac/fsw/src/fac_app.cpp @@ -35,61 +35,50 @@ #include #include -#include "fac_fac.hpp" +#include + +#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_CurrentValueTable_t CVT; -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; + + // Initialize CVT values } -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 +86,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 +98,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 +110,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 +122,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 +201,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 +215,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 +230,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 +257,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 +755,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 +765,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 +773,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 +824,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(FAC_DATA_PIPE_PEND_TIME); + 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 +868,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 pipes (0x%08X)", + "Failed to init Sch pipe (0x%08X)", (unsigned int)iStatus); goto InitApp_Exit_Tag; } - iStatus = InitHk(); + 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 Cds pipe (0x%08X)", + (unsigned int)iStatus); + goto InitApp_Exit_Tag; + } + + iStatus = objFAC.InitHk(); if (iStatus != CFE_SUCCESS) { CFE_EVS_SendEvent(FAC_INIT_ERR_EID, CFE_EVS_ERROR, @@ -421,16 +904,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 +957,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 +992,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 +1002,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..e8417c536 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,15 +51,29 @@ 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; +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_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..4b71a824f 100644 --- a/apps/fac/fsw/src/fac_cmds_task.cpp +++ b/apps/fac/fsw/src/fac_cmds_utils.cpp @@ -34,32 +34,29 @@ #include #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 +64,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 +75,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 +94,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 +158,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 +190,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 +229,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 +243,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..057e11115 --- /dev/null +++ b/apps/fac/fsw/src/fac_data_utils.cpp @@ -0,0 +1,556 @@ +/**************************************************************************** + * + * 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; + +extern FAC_CurrentValueTable_t CVT; + + +AppDataProcess::AppDataProcess() +{ +} + +AppDataProcess::~AppDataProcess() +{ +} + +void AppDataProcess::SendOutData() +{ + /* 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. */ + } +} + +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); +} + +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: + 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; + +#if 0 // check + case PX4_PARAMETER_UPDATE_MID: + HandleParameterUpdate(); + break; +#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; + + 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_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, + "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 + + 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); +} + +int32 AppDataProcess::InitDataPipe() +{ + int32 iStatus = CFE_SUCCESS; + + iStatus = CFE_SB_CreatePipe(&DataPipeId, FAC_DATA_PIPE_DEPTH, FAC_DATA_PIPE_NAME); // check depth + 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; + } +#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) + { + 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..4e2791640 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,16 @@ 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(); -typedef struct -{ - uint32 CdsTaskId; - uint32 uiCdsRunStatus; - - CriticalDataStorage *pCds; + int32 Subscribe(CFE_SB_MsgId_t MsgId); + int32 VerifyDataMsgLength(CFE_SB_Msg_t *MsgPtr, uint16 usExpectedLen); - 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/obc/fixedwing/cpd/sitl/target/CMakeLists.txt b/config/obc/fixedwing/cpd/sitl/target/CMakeLists.txt index b643ff44f..340153511 100644 --- a/config/obc/fixedwing/cpd/sitl/target/CMakeLists.txt +++ b/config/obc/fixedwing/cpd/sitl/target/CMakeLists.txt @@ -52,12 +52,6 @@ buildliner_add_app( DEFINITION ${PROJECT_SOURCE_DIR}/../private/apps/mbp_lib/fsw/for_build ) -buildliner_add_app( - sliplib - EMBEDDED - DEFINITION ${PROJECT_SOURCE_DIR}/apps/sliplib/fsw/for_build -) - buildliner_add_app( pq_lib EMBEDDED @@ -66,13 +60,19 @@ buildliner_add_app( CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/pq_lib ) +#buildliner_add_app( +# sedlib +# EMBEDDED +# DEFINITION ${PROJECT_SOURCE_DIR}/../private/apps/sedlib/fsw/for_build +# CONFIG_SOURCES +# ${PROJECT_SOURCE_DIR}/../private/config/obc/simlink/target/apps/sedlib/sedlib_shmem.c +# ${PROJECT_SOURCE_DIR}/../private/config/obc/cpd/apps/sedlib/sedlib_config.c +#) + buildliner_add_app( - sedlib + sliplib EMBEDDED - DEFINITION ${PROJECT_SOURCE_DIR}/../private/apps/sedlib/fsw/for_build - CONFIG_SOURCES - ${PROJECT_SOURCE_DIR}/../private/config/obc/simlink/target/apps/sedlib/sedlib_shmem.c - ${PROJECT_SOURCE_DIR}/../private/config/obc/cpd/apps/sedlib/sedlib_config.c + DEFINITION ${PROJECT_SOURCE_DIR}/apps/sliplib/fsw/for_build ) buildliner_add_app( @@ -87,6 +87,29 @@ buildliner_add_app( DEFINITION ${PROJECT_SOURCE_DIR}/apps/px4lib/fsw/for_build ) +buildliner_add_app( + fac + EMBEDDED + DEFINITION ${PROJECT_SOURCE_DIR}/apps/fac/fsw/for_build + CONFIG ${PROJECT_SOURCE_DIR}/config/shared/apps/fac +) + +buildliner_add_app( + cf + EMBEDDED + DEFINITION ${PROJECT_SOURCE_DIR}/apps/cf/fsw/for_build + CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/cf +) + +#buildliner_add_app( +# to +# EMBEDDED +# DEFINITION ${PROJECT_SOURCE_DIR}/apps/to/fsw/for_build +# CONFIG ${PROJECT_SOURCE_DIR}/../private/config/obc/cpd/apps/to +# CONFIG_SOURCES +# ${PROJECT_SOURCE_DIR}/../private/config/obc/cpd/apps/to/to_slip_sed.c +# ${PROJECT_SOURCE_DIR}/../private/config/obc/cpd/apps/to/tables/to_backup_cfg.c +#) # sch_custom_v1.c: Traditional platform independent layer that uses OS_Timer # to drive minor frames. @@ -105,38 +128,3 @@ buildliner_add_app( # CONFIG_SOURCES # ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/sch/sch_custom_v1.c #) - -buildliner_add_app( - cf - EMBEDDED - DEFINITION ${PROJECT_SOURCE_DIR}/apps/cf/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/cf -) - -buildliner_add_app( - ci - EMBEDDED - DEFINITION ${PROJECT_SOURCE_DIR}/apps/ci/fsw/for_build - CONFIG ${PROJECT_SOURCE_DIR}/../private/config/obc/cpd/apps/ci - CONFIG_SOURCES - ${PROJECT_SOURCE_DIR}/../private/config/obc/cpd/apps/ci/ci_slip_sed.c -) - -buildliner_add_app( - to - EMBEDDED - DEFINITION ${PROJECT_SOURCE_DIR}/apps/to/fsw/for_build - CONFIG ${PROJECT_SOURCE_DIR}/../private/config/obc/cpd/apps/to - CONFIG_SOURCES - ${PROJECT_SOURCE_DIR}/../private/config/obc/cpd/apps/to/to_slip_sed.c - ${PROJECT_SOURCE_DIR}/../private/config/obc/cpd/apps/to/tables/to_backup_cfg.c -) - -buildliner_add_app( - fac - 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 -) diff --git a/config/obc/fixedwing/cpd/sitl/target/cfe_es_startup.scr b/config/obc/fixedwing/cpd/sitl/target/cfe_es_startup.scr index b52b97ed9..db6dac483 100644 --- a/config/obc/fixedwing/cpd/sitl/target/cfe_es_startup.scr +++ b/config/obc/fixedwing/cpd/sitl/target/cfe_es_startup.scr @@ -1,14 +1,12 @@ CFE_LIB, /embedded/CFS_LIB.so, CFS_LibInit, CFS_LIB, 0, 0, 0x0, 0, 0; CFE_LIB, /embedded/MBP_LIB.so, MBP_LibInit, MBP_LIB, 0, 0, 0x0, 0, 0; CFE_LIB, /embedded/PQ_LIB.so, PQ_LibInit, PQ_LIB, 0, 0, 0x0, 0, 0; -CFE_LIB, /embedded/SED_LIB.so, SEDLIB_LibInit, SED_LIB, 0, 0, 0x0, 0, 0; CFE_LIB, /embedded/SLIP_LIB.so, SLIP_LibInit, SLIP_LIB, 0, 0, 0x0, 0, 0; CFE_LIB, /embedded/IO_LIB.so, IO_LibInit, IO_LIB, 0, 0, 0x0, 0, 0; CFE_LIB, /embedded/PX4LIB.so, PX4LIB_LibInit, PX4LIB, 0, 0, 0x0, 0, 2; CFE_APP, /embedded/FAC.so, FAC_AppMain, FAC, 20, 32768, 0x0, 0, 0; CFE_APP, /embedded/CF.so, CF_AppMain, CF, 110, 32768, 0x0, 0, 0; -CFE_APP, /embedded/CI.so, CI_AppMain, CI, 115, 32768, 0x0, 0, 0; -CFE_APP, /embedded/TO.so, TO_AppMain, TO, 124, 32768, 0x0, 0, 2; +!CFE_LIB, /embedded/SED_LIB.so, SEDLIB_LibInit, SED_LIB, 0, 0, 0x0, 0, 0; ! ! Startup script fields: ! 1. Object Type -- CFE_APP for an Application, or CFE_LIB for a library. diff --git a/config/obc/fixedwing/ppd/cfe_es_startup.scr b/config/obc/fixedwing/ppd/cfe_es_startup.scr index 5f4c572fb..601a6c7c1 100644 --- a/config/obc/fixedwing/ppd/cfe_es_startup.scr +++ b/config/obc/fixedwing/ppd/cfe_es_startup.scr @@ -1,31 +1,6 @@ CFE_LIB, /cf/apps/CFS_LIB.so, CFS_LibInit, CFS_LIB, 0, 0, 0x0, 0, 2; -CFE_LIB, /cf/apps/MBP_LIB.so, MBP_LibInit, MBP_LIB, 0, 0, 0x0, 0, 2; CFE_LIB, /cf/apps/PX4LIB.so, PX4LIB_LibInit, PX4LIB, 0, 0, 0x0, 0, 2; CFE_LIB, /cf/apps/IO_LIB.so, IO_LibInit, IO_LIB, 0, 0, 0x0, 0, 0; -CFE_LIB, /cf/apps/IPC_LIB.so, IPC_LibInit, IPC_LIB, 0, 0, 0x0, 0, 0; -CFE_APP, /cf/apps/CI.so, CI_AppMain, CI, 115, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/TO.so, TO_AppMain, TO, 124, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/DS.so, DS_AppMain, DS, 190, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/FM.so, FM_AppMain, FM, 163, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/HK.so, HK_AppMain, HK, 166, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/LC.so, LC_AppMain, LC, 178, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/SC.so, SC_AppMain, SC, 175, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/MD.so, MD_AppMain, MD, 169, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/EA.so, EA_AppMain, EA, 184, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/BAT.so, BAT_AppMain, BAT, 106, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/CS.so, CS_AppMain, CS, 160, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/SBND.so, SBND_AppMain, SBND, 37, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/SCH.so, SCH_AppMain, SCH, 34, 40960, 0x0, 0, 2; -CFE_APP, /cf/apps/ADSBR.so, ADSBR_AppMain, ADSBR, 150, 32768, 0x0, 0, 2; -!CFE_APP, /cf/apps/SBN.so, SBN_AppMain, SBN, 40, 32768, 0x0, 0, 2; -!CFE_LIB, /cf/apps/schlib.so, SCH_LibInit, SCH_LIB, 0, 0, 0x0, 0, 2; -CFE_APP, /cf/apps/CF.so, CF_AppMain, CF, 157, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/LC.so, LC_AppMain, LC, 178, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/SC.so, SC_AppMain, SC, 175, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/FLOW.so, FLOW_AppMain, FLOW, 80, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/HS.so, HS_AppMain, HS, 123, 32768, 0x0, 0, 2; -!CFE_APP, /cf/apps/ETA.so, ETA_AppMain, ETA, 100, 32768, 0x0, 0, 2; -!CFE_APP, /cf/apps/SBN.so, SBN_AppMain, SBN, 37, 32768, 0x0, 0, 2; ! ! Startup script fields: ! 1. Object Type -- CFE_APP for an Application, or CFE_LIB for a library. diff --git a/config/obc/fixedwing/ppd/sitl/target/CMakeLists.txt b/config/obc/fixedwing/ppd/sitl/target/CMakeLists.txt index 20ea0afb8..d6d169d8a 100644 --- a/config/obc/fixedwing/ppd/sitl/target/CMakeLists.txt +++ b/config/obc/fixedwing/ppd/sitl/target/CMakeLists.txt @@ -35,125 +35,3 @@ buildliner_add_app( io_lib DEFINITION ${PROJECT_SOURCE_DIR}/apps/io_lib/fsw/for_build ) - -#buildliner_add_app( -# pq_lib -# DEFINITION ${PROJECT_SOURCE_DIR}/apps/pq_lib/fsw/for_build -# # TODO move this table build to sbn module? -# CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/pq_lib -#) - -buildliner_add_app( - sch - DEFINITION ${PROJECT_SOURCE_DIR}/apps/sch/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/sch - CONFIG_SOURCES - ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/sch/sch_custom_rt.c -) - -buildliner_add_app( - cf - DEFINITION ${PROJECT_SOURCE_DIR}/apps/cf/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/cf -) - -buildliner_add_app( - cs - DEFINITION ${PROJECT_SOURCE_DIR}/apps/cs/fsw/for_build - CONFIG ${PROJECT_SOURCE_DIR}/config/shared/apps/cs -) - -buildliner_add_app( - ds - DEFINITION ${PROJECT_SOURCE_DIR}/apps/ds/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/ds -) - -buildliner_add_app( - fm - DEFINITION ${PROJECT_SOURCE_DIR}/apps/fm/fsw/for_build - CONFIG ${PROJECT_SOURCE_DIR}/config/shared/apps/fm -) - -buildliner_add_app( - hk - DEFINITION ${PROJECT_SOURCE_DIR}/apps/hk/fsw/for_build - CONFIG ${PROJECT_SOURCE_DIR}/config/shared/apps/hk -) - -buildliner_add_app( - hs - DEFINITION ${PROJECT_SOURCE_DIR}/apps/hs/fsw/for_build - CONFIG ${PROJECT_SOURCE_DIR}/config/shared/apps/hs - CONFIG_SOURCES - ${PROJECT_SOURCE_DIR}/config/shared/apps/hs/hs_custom.c -) - -buildliner_add_app( - lc - DEFINITION ${PROJECT_SOURCE_DIR}/apps/lc/fsw/for_build - CONFIG ${PROJECT_SOURCE_DIR}/config/shared/apps/lc - CONFIG_SOURCES - ${PROJECT_SOURCE_DIR}/config/shared/apps/lc/src/lc_custom.c -) - -buildliner_add_app( - md - DEFINITION ${PROJECT_SOURCE_DIR}/apps/md/fsw/for_build - CONFIG ${PROJECT_SOURCE_DIR}/config/shared/apps/md -) - -buildliner_add_app( - mm - DEFINITION ${PROJECT_SOURCE_DIR}/apps/mm/fsw/for_build -) - -buildliner_add_app( - sc - DEFINITION ${PROJECT_SOURCE_DIR}/apps/sc/fsw/for_build - CONFIG ${PROJECT_SOURCE_DIR}/config/shared/apps/sc -) - -buildliner_add_app( - ci - DEFINITION ${PROJECT_SOURCE_DIR}/apps/ci/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/ci - CONFIG_SOURCES - ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/ci/ci_custom.c -) - -buildliner_add_app( - to - DEFINITION ${PROJECT_SOURCE_DIR}/apps/to/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/to - CONFIG_SOURCES - ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/to/tables/to_backup_cfg.c - ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/to/to_custom.c -) - -buildliner_add_app( - ea - DEFINITION ${PROJECT_SOURCE_DIR}/apps/ea/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/ea - CONFIG_SOURCES - ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/ea/src/ea_custom.c -) - -#buildliner_add_app( -# sbn -# DEFINITION ${PROJECT_SOURCE_DIR}/apps/sbn/fsw/for_build -# CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/sbn -# CONFIG_SOURCES -# ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/sbn/src/sbn_remap_tbl.c -#) - -#buildliner_add_app( -# sbn_udp -# DEFINITION ${PROJECT_SOURCE_DIR}/apps/sbn/modules/udp/fsw/for_build -#) - -buildliner_add_app( - adsbr - DEFINITION ${PROJECT_SOURCE_DIR}/apps/adsbr/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/../../apps/adsbr -) diff --git a/config/obc/fixedwing/simlink/target/CMakeLists.txt b/config/obc/fixedwing/simlink/target/CMakeLists.txt index 9d531ca4e..3977525df 100644 --- a/config/obc/fixedwing/simlink/target/CMakeLists.txt +++ b/config/obc/fixedwing/simlink/target/CMakeLists.txt @@ -29,109 +29,13 @@ buildliner_add_app( DEFINITION ${PROJECT_SOURCE_DIR}/apps/cfs_lib/fsw/for_build ) - buildliner_add_app( cvt DEFINITION ${PROJECT_SOURCE_DIR}/apps/cvt/fsw/for_build ) - buildliner_add_app( simlink DEFINITION ${PROJECT_SOURCE_DIR}/apps/simlink/gazebo9/for_build CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/apps/simlink ) - - -buildliner_add_app( - sch - DEFINITION ${PROJECT_SOURCE_DIR}/apps/sch/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/apps/sch - CONFIG_SOURCES - ${CMAKE_CURRENT_SOURCE_DIR}/apps/sch/sch_custom_rt.c -) - - -buildliner_add_app( - ci - DEFINITION ${PROJECT_SOURCE_DIR}/apps/ci/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/apps/ci - CONFIG_SOURCES - ${CMAKE_CURRENT_SOURCE_DIR}/apps/ci/ci_custom.c -) - - -buildliner_add_app( - to - DEFINITION ${PROJECT_SOURCE_DIR}/apps/to/fsw/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/apps/to - CONFIG_SOURCES - ${CMAKE_CURRENT_SOURCE_DIR}/apps/to/tables/to_backup_cfg.c - ${CMAKE_CURRENT_SOURCE_DIR}/apps/to/to_custom.c -) - - -buildliner_add_app( - sedlib - DEFINITION ${PROJECT_SOURCE_DIR}/../private/apps/sedlib/fsw/for_build - CONFIG_SOURCES - ${PROJECT_SOURCE_DIR}/../private/config/obc/simlink/target/apps/sedlib/sedlib_config.c - ${PROJECT_SOURCE_DIR}/../private/config/obc/simlink/target/apps/sedlib/sedlib_shmem.c -) - - -buildliner_add_app( - ublox_m8n - DEFINITION ${PROJECT_SOURCE_DIR}/../private/apps/gps/simlink/ublox_m8n/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/apps/ublox_m8n -) - - -buildliner_add_app( - hmc5883 - DEFINITION ${PROJECT_SOURCE_DIR}/../private/apps/hmc5883/simlink/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/apps/hmc5883 -) - - -buildliner_add_app( - icm20689 - DEFINITION ${PROJECT_SOURCE_DIR}/../private/apps/icm20689/simlink/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/apps/icm20689 -) - - -buildliner_add_app( - ms5611 - DEFINITION ${PROJECT_SOURCE_DIR}/apps/ms5611/simlink/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/apps/ms5611 -) - - -buildliner_add_app( - rfd900x - DEFINITION ${PROJECT_SOURCE_DIR}/apps/rfd900x/simlink/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/apps/rfd900x -) - - -buildliner_add_app( - sedpwm - DEFINITION ${PROJECT_SOURCE_DIR}/../private/apps/sedpwm/simlink/for_build - CONFIG ${CMAKE_CURRENT_SOURCE_DIR}/apps/sedpwm -) - - -buildliner_add_app( - sbus - DEFINITION ${PROJECT_SOURCE_DIR}/../private/apps/sedsbus/simlink/for_build - CONFIG ${PROJECT_SOURCE_DIR}/../private/config/obc/simlink/target/apps/sbus - CONFIG_SOURCES - ${PROJECT_SOURCE_DIR}/../private/config/obc/simlink/target/apps/sbus/sbus_custom.c -) - -buildliner_add_app( - sed - DEFINITION ${PROJECT_SOURCE_DIR}/../private/apps/sed/simlink/for_build - CONFIG ${PROJECT_SOURCE_DIR}/../private/config/obc/simlink/target/apps/sed -) diff --git a/config/obc/fixedwing/simlink/target/cfe_es_startup.scr b/config/obc/fixedwing/simlink/target/cfe_es_startup.scr index 78b0c60ec..787595d1b 100644 --- a/config/obc/fixedwing/simlink/target/cfe_es_startup.scr +++ b/config/obc/fixedwing/simlink/target/cfe_es_startup.scr @@ -1,20 +1,6 @@ CFE_LIB, /cf/apps/CFS_LIB.so, CFS_LibInit, CFS_LIB, 0, 0, 0x0, 0, 2; -CFE_LIB, /cf/apps/CVTLIB.so, CVT_LibInit, CVTLIB, 0, 0, 0x0, 0, 2; -CFE_LIB, /cf/apps/SED_LIB.so, SEDLIB_LibInit, SED_LIB, 0, 0, 0x0, 0, 2; -CFE_APP, /cf/apps/SED.so, SED_AppMain, SED, 127, 65536, 0x0, 0, 0; -CFE_APP, /cf/apps/CI.so, CI_AppMain, CI, 115, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/TO.so, TO_AppMain, TO, 124, 327680, 0x0, 0, 2; -CFE_APP, /cf/apps/UBLOX_M8N.so, UBLOX_M8N_AppMain, UBLOX_M8N, 50, 40960, 0x0, 0, 2; -CFE_APP, /cf/apps/HMC5883.so, HMC5883_AppMain, HMC5883, 53, 40960, 0x0, 0, 2; -CFE_APP, /cf/apps/ICM20689.so, ICM20689_AppMain, ICM20689, 56, 40960, 0x0, 0, 2; -CFE_APP, /cf/apps/RFD900X.so, RFD900X_AppMain, RFD900X, 62, 40960, 0x0, 0, 2; -CFE_APP, /cf/apps/MS5611.so, MS5611_AppMain, MS5611, 65, 40960, 0x0, 0, 2; -CFE_APP, /cf/apps/SEDPWM.so, SEDPWM_AppMain, SEDPWM, 88, 40960, 0x0, 0, 2; -CFE_APP, /cf/apps/SBUS.so, SBUS_AppMain, SBUS, 91, 40960, 0x0, 0, 2; CFE_APP, /cf/apps/SIMLINK.so, SIMLINK_AppMain, SIMLINK, 45, 40960, 0x0, 0, 2; -CFE_APP, /cf/apps/SCH.so, SCH_AppMain, SCH, 34, 40960, 0x0, 0, 2; ! -CFE_LIB, /cf/apps/SED_LIB.so, SEDLIB_LibInit, SED_LIB, 0, 0, 0x0, 0, 0; ! Startup script fields: ! 1. Object Type -- CFE_APP for an Application, or CFE_LIB for a library. ! 2. Path/Filename -- This is a cFE Virtual filename, not a vxWorks device/pathname 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 + + diff --git a/core/base/tools/commander/workspace_template/bin/wh_defs.yaml b/core/base/tools/commander/workspace_template/bin/wh_defs.yaml index b821ea341..035165316 100644 --- a/core/base/tools/commander/workspace_template/bin/wh_defs.yaml +++ b/core/base/tools/commander/workspace_template/bin/wh_defs.yaml @@ -2713,7 +2713,7 @@ modules: value: FAC_CMD_PIPE FAC_DATA_PIPE_DEPTH: name: FAC_DATA_PIPE_DEPTH - value: 5 + value: 4 FAC_DATA_PIPE_NAME: name: FAC_DATA_PIPE_NAME value: FAC_DATA_PIPE