├── .gitmodules ├── doc ├── catlas-hw.png ├── eswbmon.png ├── catlas-flow.png ├── catlas-swsys.png ├── logo_discord.png └── catpilot-structure.jpg ├── xml_inline.h ├── .clang-format ├── .gitignore ├── .catlas_prj.xml ├── .vscode ├── c_cpp_properties.json ├── cmake-variants.json ├── settings.json ├── tasks.json └── launch.json ├── config └── quad │ ├── flow_cont_odrive.xml │ ├── flow_ublox_parsing.xml │ ├── flow_housekeeping.xml │ ├── flow_nav_vel_pos_fusion.xml │ ├── flow_nav_imu_alignment.xml │ ├── flow_nav_gnss_process.xml │ ├── flow_nav_attitude_fusion.xml │ ├── flow_nav_attitude_prop.xml │ ├── flow_nav_compass.xml │ ├── flow_cont_angpos.xml │ ├── ibr_ubx.xml │ ├── flow_nav_vel_pos_prop.xml │ ├── flow_cont_vel_pos.xml │ ├── flow_rc.xml │ ├── flow_cont_angrate.xml │ └── swsys.xml ├── makefile ├── LICENSE ├── xml_inline.c ├── main.c ├── system_schematic.xml ├── CMakeLists.txt ├── test_udp_gui.py ├── README.md └── gui.py /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "catpilot"] 2 | path = catpilot 3 | url = ../catpilot 4 | -------------------------------------------------------------------------------- /doc/catlas-hw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ctlst-tech/uas-catpilot/HEAD/doc/catlas-hw.png -------------------------------------------------------------------------------- /doc/eswbmon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ctlst-tech/uas-catpilot/HEAD/doc/eswbmon.png -------------------------------------------------------------------------------- /doc/catlas-flow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ctlst-tech/uas-catpilot/HEAD/doc/catlas-flow.png -------------------------------------------------------------------------------- /doc/catlas-swsys.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ctlst-tech/uas-catpilot/HEAD/doc/catlas-swsys.png -------------------------------------------------------------------------------- /doc/logo_discord.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ctlst-tech/uas-catpilot/HEAD/doc/logo_discord.png -------------------------------------------------------------------------------- /doc/catpilot-structure.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ctlst-tech/uas-catpilot/HEAD/doc/catpilot-structure.jpg -------------------------------------------------------------------------------- /xml_inline.h: -------------------------------------------------------------------------------- 1 | #ifndef XML_INLINE_H 2 | #define XML_INLINE_H 3 | 4 | int xml_inline_mount(const char *mount_to); 5 | 6 | #endif // XML_INLINE_H 7 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | IndentWidth: 4 3 | ColumnLimit: 80 4 | AlignAfterOpenBracket: Align 5 | AllowShortFunctionsOnASingleLine: Inline 6 | AccessModifierOffset: -4 7 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | build/ 3 | atomics_reg.c 4 | atomics.cmake 5 | xml_inline_cfgs.c 6 | bblocks.xml 7 | __pycache__ 8 | .idea/ 9 | cmake-build-*/ 10 | .cortex-debug.peripherals.state.json 11 | .cortex-debug.registers.state.json 12 | .DS_Store 13 | -------------------------------------------------------------------------------- /.catlas_prj.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "mcu", 5 | "compilerPath": "/usr/bin/arm-none-eabi-gcc", 6 | "includePath": [ 7 | "${workspaceRoot}/**" 8 | ], 9 | "cStandard": "c99", 10 | "intelliSenseMode": "gcc-x64", 11 | "configurationProvider": "ms-vscode.cmake-tools" 12 | } 13 | ], 14 | "version": 4 15 | } -------------------------------------------------------------------------------- /.vscode/cmake-variants.json: -------------------------------------------------------------------------------- 1 | { 2 | "buildType": { 3 | "default": "Debug", 4 | "description": "The build type", 5 | "choices": { 6 | "Debug": { 7 | "short": "Debug", 8 | "long": "Debug: with debug info", 9 | "buildType": "Debug" 10 | }, 11 | "Release": { 12 | "short": "Release", 13 | "long": "Release: no debug info", 14 | "buildType": "Release" 15 | } 16 | } 17 | } 18 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "cmake.setBuildTypeOnMultiConfig": true, 3 | "cmake.configureSettings": { 4 | "BOARD": "cube", 5 | "CLI_PORT": "TELEM2", 6 | "CLI_BAUDRATE": "57600", 7 | "OS_MONITOR": "ON", 8 | "MAINTENANCE_MODE": "OFF" 9 | }, 10 | "cortex-debug.armToolchainPath": "/usr/bin", 11 | "cortex-debug.openocdPath": "/usr/local/bin/openocd", 12 | "cortex-debug.gdbPath": "/usr/bin/arm-none-eabi-gdb", 13 | "openocd_path": "/usr/local/share/openocd", 14 | "python.analysis.extraPaths": [ 15 | "./catpilot/c-atom/eswb/pytools" 16 | ], 17 | } 18 | -------------------------------------------------------------------------------- /.vscode/tasks.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2.0.0", 3 | "tasks": [ 4 | { 5 | "label": "Flash", 6 | "type": "shell", 7 | "command": "/usr/local/bin/openocd -s ${config:openocd_path}/scripts -f interface/stlink.cfg -f ./catpilot/bsp/mcu/core/stm32/h753/stm32h753.cfg -c \"init\" -c \"program ./build/firmware/uas-catpilot.elf verify reset exit\"", 8 | "problemMatcher": [], 9 | "group": { 10 | "kind": "build", 11 | "isDefault": true 12 | } 13 | }, 14 | { 15 | "label": "Erase", 16 | "type": "shell", 17 | "command": "st-flash erase", 18 | "problemMatcher": [] 19 | } 20 | ] 21 | } 22 | -------------------------------------------------------------------------------- /config/quad/flow_cont_odrive.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | inputs/pos 18 | can1 19 | 0 20 | 21 | 22 | 23 | 24 | odrive/vol 25 | odrive/cur 26 | 27 | 28 | -------------------------------------------------------------------------------- /config/quad/flow_ublox_parsing.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 |

7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 256 18 | 256 19 | $serial_path 20 | 115200 21 | 22 | 23 | 24 | 25 | rx_frame_sync/ubx_frame 26 | rx_frame_sync/rtcm_frame 27 | 28 | 29 | -------------------------------------------------------------------------------- /makefile: -------------------------------------------------------------------------------- 1 | 2 | atomics_flight: 3 | @echo Generating atomics functions code 4 | @./catpilot/c-atom/tools/fspecgen.py --catom_path catpilot/c-atom --code --cmake --registry_c ./atomics_reg.c --atomics_dirs cube:catpilot/atomics/cube ublox:catpilot/atomics/ublox odrive:catpilot/atomics/odrive catom:catpilot/c-atom/atomics 5 | 6 | atomics_ground: 7 | @./catpilot/c-atom/tools/fspecgen.py --catom_path catpilot/c-atom --code --cmake --registry_c ./atomics_reg.c --atomics_dirs catpilot:catpilot/atomics/ublox catom:catpilot/c-atom/atomics 8 | 9 | xmlinline: 10 | @echo Inlining XML configs 11 | @./catpilot/c-atom/tools/xml2c_inliner.py --cfg_path config/quad/ --out xml_inline_cfgs.c 12 | 13 | bblocks: 14 | @./catpilot/c-atom/tools/fspecgen.py --catom_path catpilot/c-atom --code --cmake --bbxml bblocks.xml --atomics_dirs catpilot:catpilot/atomics/ catom:catpilot/c-atom/atomics/ 15 | 16 | clean_build: 17 | @echo Building 18 | rm -r -f build 19 | mkdir build 20 | cd build && cmake .. -DBOARD=cube -DCMAKE_BUILD_TYPE=Release && make uas-catpilot.elf -j15 21 | 22 | cube: 23 | rm -r -f build && mkdir build && cd build && cmake .. -DBOARD=cube -DCLI_PORT=DBG -DCLI_BAUDRATE=115200 -DOS_MONITOR=ON -DCMAKE_BUILD_TYPE=Release && make uas-catpilot.elf 24 | 25 | flash: 26 | @echo Firmware downloading 27 | openocd -f interface/stlink.cfg -f ./catpilot/bsp/mcu/core/stm32/h753/stm32h753.cfg -c "init" -c "program ./build/firmware/uas-catpilot.elf verify reset exit" 28 | -------------------------------------------------------------------------------- /config/quad/flow_housekeeping.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 1.0 19 | 20 | 21 | 0 22 | 15.3 23 | 24 | 25 | 1 26 | 50.0 27 | 0.45 28 | 29 | 30 | 31 | 32 | sine/output 33 | voltage/output 34 | voltage/min 35 | voltage/max 36 | current/output 37 | current/min 38 | current/max 39 | 40 | 41 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, Catalyst Aerospace Technologies 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | -------------------------------------------------------------------------------- /config/quad/flow_nav_vel_pos_fusion.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 0.05 22 | 23 | 24 | 25 | inputs/vel 26 | inputs/gnss_vel 27 | 28 | 29 | 30 | vel_gain/output 31 | vel_residual/v 32 | 33 | 34 | 35 | inputs/pos 36 | inputs/gnss_pos 37 | 38 | 39 | 40 | 0.025 41 | 42 | 43 | 44 | pos_gain/output 45 | pos_residual/v 46 | 47 | 48 | 49 | 50 | vel_compensation/v 51 | pos_compensation/v 52 | 53 | 54 | -------------------------------------------------------------------------------- /config/quad/flow_nav_imu_alignment.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | inputs/omega/x 21 | inputs/omega/y 22 | inputs/omega/z 23 | 24 | 25 | 26 | bias_calib_proto/a1 27 | bias_calib_proto/a2 28 | bias_calib_proto/a3 29 | 30 | 31 | 32 | inputs/a 33 | 34 | 35 | 36 | 0.0 37 | 38 | 39 | 40 | accel_horizon/roll 41 | accel_horizon/pitch 42 | initial_yaw/output 43 | 44 | 45 | 46 | bias_calib_proto/filled 47 | 48 | 49 | 50 | 51 | omega_bias_concat/v 52 | initial_quat/q 53 | imu_reset/output 54 | 55 | 56 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "configurations": [ 4 | {"name":"Python: Current File","type":"python","request":"launch","program":"${file}","console":"integratedTerminal","justMyCode":true}, 5 | { 6 | "name": "Cube", 7 | "request": "launch", 8 | "type": "cortex-debug", 9 | "cwd": "${workspaceRoot}", 10 | "servertype": "openocd", 11 | "executable": "build/firmware/uas-catpilot.elf", 12 | "svdFile": "catpilot/bsp/mcu/core/stm32/h753/STM32H753.svd", 13 | "searchDir": [ 14 | "/usr/local/share/openocd/scripts",], 15 | "configFiles": [ 16 | "catpilot/bsp/mcu/core/stm32/h753/stm32h753.cfg" 17 | ], 18 | }, 19 | { 20 | "name": "Host", 21 | "request": "launch", 22 | "type": "cppdbg", 23 | "cwd": "${workspaceRoot}", 24 | "args": ["config/ctlst/swsys.xml"], 25 | "program": "${workspaceRoot}/build/firmware/catom-launcher", 26 | "miDebuggerPath": "/usr/bin/gdb", 27 | "MIMode": "gdb", 28 | "setupCommands": [ 29 | { 30 | "description": "Enable pretty-printing for gdb", 31 | "text": "-enable-pretty-printing", 32 | "ignoreFailures": true 33 | } 34 | ] 35 | }, 36 | { 37 | "name": "eswbutil_convert_to_file", 38 | "request": "launch", 39 | "type": "cppdbg", 40 | "cwd": "${workspaceRoot}", 41 | "args": ["--convert_to_csv", "/tmp/sin_0.eqrb", "./sin_0.csv"], 42 | "program": "${workspaceRoot}/catpilot/c-atom/eswb/build/eswbutil", 43 | "miDebuggerPath": "/usr/bin/gdb", 44 | "MIMode": "gdb", 45 | "setupCommands": [ 46 | { 47 | "description": "Enable pretty-printing for gdb", 48 | "text": "-enable-pretty-printing", 49 | "ignoreFailures": true 50 | } 51 | ] 52 | }, 53 | ] 54 | } 55 | -------------------------------------------------------------------------------- /config/quad/flow_nav_gnss_process.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 10 27 | 28 | 29 | 30 | inputs/prec 31 | prec_threshold/output 32 | 33 | 34 | 35 | fix_stub/output 36 | prec_is_ok/output 37 | 38 | 39 | 40 | 41 | reset_cmd/output 42 | ready/output 43 | 44 | 45 | 46 | inputs/lat 47 | inputs/lon 48 | inputs/alt 49 | 50 | 51 | 52 | inputs/vel_d 53 | 54 | 55 | 56 | inputs/vel_n 57 | inputs/vel_e 58 | ven_u/output 59 | 60 | 61 | 62 | 63 | ready_reg/output 64 | concat_vel/v 65 | concat_pos/v 66 | 67 | 68 | -------------------------------------------------------------------------------- /xml_inline.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "node.h" 7 | 8 | typedef struct { 9 | uint8_t *file_str; 10 | size_t file_len; 11 | size_t offset; 12 | } xml_inline_ocb_t; // open context block 13 | 14 | /** 15 | * This call is generated by the c-atom's tool xml2c_inliner.py 16 | * simply run 'make prebuild' 17 | * @param path 18 | * @param size 19 | * @return 20 | */ 21 | const char *xml_inline_find_file(const char *path, size_t *size); 22 | 23 | static int xml_inline_open_callback(FILE *file, const char *path) { 24 | size_t filesize; 25 | 26 | if (file->private_data == NULL) { 27 | file->private_data = calloc(1, sizeof(xml_inline_ocb_t)); 28 | } 29 | 30 | xml_inline_ocb_t *ocb = (xml_inline_ocb_t *)file->private_data; 31 | 32 | const char *file_string = xml_inline_find_file(path, &filesize); 33 | 34 | if (file_string == NULL) { 35 | errno = ENOENT; 36 | return -1; 37 | } 38 | // check size 39 | #define MAXLEN (filesize + 100) 40 | ocb->file_len = strnlen(file_string, MAXLEN); 41 | if (ocb->file_len >= MAXLEN) { 42 | errno = EFBIG; 43 | return -1; 44 | } 45 | if (ocb->file_len != filesize) { 46 | errno = EIO; 47 | return -1; 48 | } 49 | 50 | ocb->file_str = (uint8_t *)file_string; 51 | 52 | ocb->offset = 0; 53 | 54 | return 0; 55 | } 56 | 57 | static ssize_t xml_inline_read_callback(FILE *file, char *buf, size_t count) { 58 | xml_inline_ocb_t *ocb = (xml_inline_ocb_t *)file->private_data; 59 | 60 | size_t bytes_left = ocb->file_len - ocb->offset; 61 | ssize_t br = count > bytes_left ? bytes_left : count; 62 | 63 | if (br > 0) { 64 | memcpy(buf, ocb->file_str + ocb->offset, br); 65 | ocb->offset += br; 66 | } 67 | 68 | errno = 0; 69 | 70 | return br; 71 | } 72 | 73 | static int xml_inline_close_callback(FILE *file) { 74 | xml_inline_ocb_t *ocb = (xml_inline_ocb_t *)file->private_data; 75 | // TODO 76 | return 0; 77 | } 78 | 79 | int xml_inline_mount(const char *mount_to) { 80 | struct file_operations f_op = {.open = xml_inline_open_callback, 81 | .write = NULL, 82 | .read = xml_inline_read_callback, 83 | .close = xml_inline_close_callback, 84 | .dev = NULL}; 85 | struct node *node = node_mount(mount_to, &f_op); 86 | return 0; 87 | } 88 | -------------------------------------------------------------------------------- /config/quad/flow_nav_attitude_fusion.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | inputs/a 22 | 23 | 24 | 25 | inputs/q 26 | 27 | 28 | 29 | to_euler/roll 30 | accel_horizon/roll 31 | 32 | 33 | 34 | to_euler/pitch 35 | accel_horizon/pitch 36 | 37 | 38 | 39 | to_euler/yaw 40 | inputs/azimuth 41 | 42 | 43 | 44 | roll_err/output 45 | 46 | 0.0005 47 | 48 | 49 | 50 | pitch_err/output 51 | 52 | 0.0005 53 | 54 | 55 | 56 | yaw_err/output 57 | 58 | 0.0005 59 | 60 | 61 | 62 | inputs/q 63 | 64 | 65 | 66 | yaw_adjustment/output 67 | pitch_adjustment/output 68 | roll_adjustment/output 69 | 70 | 71 | 72 | 73 | euler_corrections/v 74 | to_euler_out/roll 75 | to_euler_out/pitch 76 | to_euler_out/yaw 77 | 78 | 79 | -------------------------------------------------------------------------------- /main.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "board.h" 4 | #include "cli.h" 5 | #include "swsys.h" 6 | #include "xml_inline.h" 7 | 8 | swsys_t core_sys; 9 | int catpilot(void); 10 | 11 | #define SWSYS_COMMANDER_MAX_CMD_LENGTH 128 12 | static char swsys_cmd[SWSYS_COMMANDER_MAX_CMD_LENGTH]; 13 | static char *swsys_cmd_offset; 14 | static swsys_t *swsys_commander_ptr; 15 | 16 | swsys_rv_t swsys_commander_init(swsys_t *swsys) { 17 | if (swsys != NULL) { 18 | swsys_commander_ptr = swsys; 19 | return swsys_e_ok; 20 | } 21 | return swsys_e_invargs; 22 | } 23 | 24 | int swsys_commander(int argc, char **argv) { 25 | if (argc < 3) { 26 | dbg_msg("Usage: swsys [task] func_name=[func] [param_alias]=[value]"); 27 | return 0; 28 | } 29 | 30 | if (swsys_commander_ptr == NULL) { 31 | dbg_msg("swsys_commander is not initialized"); 32 | return 0; 33 | } 34 | 35 | swsys_cmd_offset = swsys_cmd; 36 | for (int i = 2; i < argc; i++) { 37 | if (swsys_cmd_offset > swsys_cmd + SWSYS_COMMANDER_MAX_CMD_LENGTH) { 38 | dbg_msg("Command exceeds max length"); 39 | return 0; 40 | } 41 | strncpy(swsys_cmd_offset, argv[i], 42 | swsys_cmd + SWSYS_COMMANDER_MAX_CMD_LENGTH - swsys_cmd_offset); 43 | swsys_cmd_offset += strnlen(argv[i], SWSYS_COMMANDER_MAX_CMD_LENGTH); 44 | swsys_cmd_offset[0] = ' '; 45 | swsys_cmd_offset++; 46 | } 47 | 48 | swsys_rv_t rv = swsys_set_params(swsys_commander_ptr, argv[1], swsys_cmd); 49 | 50 | if (rv != swsys_e_ok) { 51 | dbg_msg("Wrong command"); 52 | dbg_msg("Usage: swsys [task] func_name=[func] [param_alias]=[value]"); 53 | } else { 54 | dbg_msg("Command accepted"); 55 | } 56 | 57 | return 0; 58 | } 59 | 60 | int main(void) { 61 | board_start(catpilot, 8192, CLI_PORT, CLI_BAUDRATE); 62 | while (1) { 63 | } 64 | } 65 | 66 | int catpilot(void) { 67 | pthread_setname_np((char *)__func__); 68 | xml_inline_mount("/cfg"); 69 | cli_cmd_reg("swsys", swsys_commander); 70 | swsys_rv_t swsys_rv = swsys_load("/cfg/swsys.xml", "/cfg", &core_sys); 71 | swsys_rv_t swsys_cmd_rv = swsys_commander_init(&core_sys); 72 | if (swsys_rv == swsys_e_ok && swsys_cmd_rv == swsys_e_ok) { 73 | printf("SWSYS \"%s\" loaded\n", 74 | core_sys.name != NULL ? core_sys.name : "no name"); 75 | LOG_INFO("SYSTEM", "Configuration loading successful"); 76 | swsys_rv = swsys_top_module_start(&core_sys); 77 | if (swsys_rv != swsys_e_ok) { 78 | LOG_ERROR("SYSTEM", "Module start error"); 79 | printf("SWSYS \"%s\" failed to start\n", 80 | core_sys.name != NULL ? core_sys.name : "no name"); 81 | } 82 | } else { 83 | LOG_ERROR("SYSTEM", "Configuration loading error"); 84 | } 85 | return -1; 86 | } 87 | -------------------------------------------------------------------------------- /system_schematic.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /config/quad/flow_nav_attitude_prop.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | imu1/ax 25 | 26 | 5 27 | 28 | 29 | 30 | imu1/ay 31 | 32 | 5 33 | 34 | 35 | 36 | imu1/az 37 | 38 | 5 39 | 40 | 41 | 42 | imu1/wx 43 | inputs/omega_bias/x 44 | 45 | 46 | 47 | imu1/wy 48 | inputs/omega_bias/y 49 | 50 | 51 | 52 | imu1/wz 53 | inputs/omega_bias/z 54 | 55 | 56 | 57 | omega_x_biased/output 58 | omega_y_biased/output 59 | omega_z_biased/output 60 | 61 | 62 | 63 | inputs/roll 64 | inputs/q 65 | inputs/euler_correction/x 66 | inputs/euler_correction/y 67 | inputs/euler_correction/z 68 | 69 | 70 | 71 | correction/q 72 | omega_biased/v 73 | inputs/q0 74 | inputs/reset 75 | 76 | 77 | 78 | ax/output 79 | ay/output 80 | az/output 81 | 82 | 83 | 84 | 85 | concat_accel/v 86 | omega_biased/v 87 | integrate_att/q 88 | 89 | 90 | -------------------------------------------------------------------------------- /config/quad/flow_nav_compass.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | mag1/magz 20 | mag1/magx 21 | mag1/magy 22 | 23 | 24 | 25 | mag_concat/v 26 | 27 | 1.0 28 | 0.0 29 | 0.0 30 | 0.0 31 | 1.0 32 | 0.0 33 | 0.0 34 | 0.0 35 | 1.0 36 | -0.05 37 | -0.1 38 | +0.15 39 | 40 | 41 | 42 | inputs/q 43 | 44 | 45 | 46 | euler/roll 47 | 48 | 49 | 50 | euler/pitch 51 | 52 | 53 | 54 | 0 55 | 56 | 57 | 58 | inverse_pitch/output 59 | inverse_roll/output 60 | yaw_zero/output 61 | 62 | 63 | 64 | mag_calib/v 65 | quat4projection/q 66 | 67 | 68 | 69 | mag_calib/v 70 | 71 | 72 | 73 | magnitude/output 74 | 75 | 0.530278 76 | 77 | 78 | 79 | proj_to_horizon/v/y 80 | 81 | 82 | 83 | inverse_y/output 84 | proj_to_horizon/v/x 85 | 86 | 87 | 88 | -15 89 | 90 | 91 | 92 | local_mag_declination/output 93 | 94 | 95 | 96 | conv_declination/output 97 | mag_azimuth/output 98 | 99 | 100 | 101 | 102 | mag_azimuth_w_decl/output 103 | normalized_induction_module/output 104 | proj_to_horizon/v 105 | 106 | 107 | -------------------------------------------------------------------------------- /config/quad/flow_cont_angpos.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | inputs/desired_attitude_rc 27 | inputs/desired_attitude_auto 28 | inputs/enable_auto 29 | 30 | 31 | 32 | 0.0 33 | 34 | 35 | 36 | inputs/hold_yaw_cmd 37 | inputs/enable_angpos 38 | 39 | 40 | 41 | attitude_input_source/output/x 42 | inputs/roll 43 | angles_preset/output 44 | inputs/enable_angpos 45 | 46 | 4 47 | -0.1 48 | +0.1 49 | -0.8 50 | +0.8 51 | 52 | 53 | 54 | attitude_input_source/output/y 55 | inputs/pitch 56 | angles_preset/output 57 | inputs/enable_angpos 58 | 59 | 4 60 | -0.1 61 | +0.1 62 | -0.8 63 | +0.8 64 | 65 | 66 | 67 | attitude_input_source/output/z 68 | inputs/yaw 69 | yaw_regulation_enable/output 70 | 71 | 72 | 73 | pid_roll/output 74 | inputs/pids_gain 75 | 76 | 77 | 78 | pid_pitch/output 79 | inputs/pids_gain 80 | 81 | 82 | 83 | inputs/rc_desired_omega/z 84 | pid_yaw/output 85 | pid_yaw/enabled 86 | 87 | 88 | 89 | omega_z_mux/output 90 | pid_multed_roll/output 91 | pid_multed_pitch/output 92 | 93 | 94 | 95 | inputs/rc_desired_omega 96 | desired_auto_omega/v 97 | inputs/enable_angpos 98 | 99 | 100 | 101 | 102 | desired_omega/output 103 | pid_pitch/enabled 104 | 105 | 106 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15) 2 | 3 | set(PROJ_NAME "uas-catpilot") 4 | 5 | if (BOARD STREQUAL "host") 6 | set(MCU_FAMILY host) 7 | set(MCU_MODEL host) 8 | set(OS "posix") 9 | set(FS "posix") 10 | set(CLI_PORT "stdio") 11 | set(CLI_BAUDRATE "") 12 | elseif (BOARD STREQUAL "cube") 13 | set(MCU_FAMILY stm32) 14 | set(MCU_MODEL h753) 15 | set(IC_CUBEIO "ON") 16 | set(IC_ICM20602 "ON") 17 | set(IC_ICM20649 "ON") 18 | set(IC_ICM20948 "ON") 19 | set(IC_IST8310 "ON") 20 | set(IC_MS5611 "ON") 21 | set(IC_SD "ON") 22 | set(OS "freertos") 23 | set(FS "fatfs") 24 | 25 | set(ESWB_EQRB_NO_SOCKET "1") 26 | set(ESWB_SDTL_NO_SOCKET "1") 27 | set(ESWB_DEBUG "1") 28 | set(CATOM_NO_SOCKET "1") 29 | set(FAKE_PTHREAD "1") 30 | 31 | if (NOT DEFINED CLI_PORT) 32 | set(CLI_PORT "TELEM2") 33 | endif() 34 | if (NOT DEFINED CLI_BAUDRATE) 35 | set(CLI_BAUDRATE "115200") 36 | endif() 37 | if (OS_MONITOR STREQUAL "ON") 38 | add_definitions(-DOS_MONITOR) 39 | endif() 40 | if (MAINTENANCE_MODE STREQUAL "ON") 41 | add_definitions(-DMAINTENANCE_MODE) 42 | endif() 43 | add_definitions(-DCLI_PORT="${CLI_PORT}") 44 | add_definitions(-DCLI_BAUDRATE="${CLI_BAUDRATE}") 45 | elseif (BOARD STREQUAL "ctlst") 46 | set(MCU_FAMILY zynq) 47 | set(MCU_MODEL 7020) 48 | set(OS "qnx") 49 | set(FS "qnx") 50 | set(IC_CTLST "ON") 51 | 52 | set(ESWB_EQRB_NO_SOCKET "1") 53 | set(CATOM_NO_SOCKET "1") 54 | set(FAKE_PTHREAD "1") 55 | else () 56 | message(FATAL_ERROR "Board is not selected!") 57 | endif () 58 | 59 | execute_process ( 60 | COMMAND bash -c "git show --quiet | grep 'commit ' | sed 's/commit //' | awk '{print substr($0,1,8)}' | tr '\n' ' '" 61 | OUTPUT_VARIABLE GIT_HASH_UAS 62 | ) 63 | execute_process ( 64 | COMMAND bash -c "if [ -z \"$(git status -s)\" ]; then echo clean; else echo dirty; fi | tr '\n' ' '" 65 | OUTPUT_VARIABLE GIT_STATE_UAS 66 | ) 67 | execute_process ( 68 | COMMAND bash -c "cd ../catpilot && git show --quiet | grep 'commit ' | sed 's/commit //' | awk '{print substr($0,1,8)}' | tr '\n' ' '" 69 | OUTPUT_VARIABLE GIT_HASH_CATPILOT 70 | ) 71 | execute_process ( 72 | COMMAND bash -c "cd ../catpilot && if [ -z \"$(git status -s)\" ]; then echo clean; else echo dirty; fi | tr '\n' ' '" 73 | OUTPUT_VARIABLE GIT_STATE_CATPILOT 74 | ) 75 | 76 | add_definitions(-DGIT_HASH_UAS="${GIT_HASH_UAS}") 77 | add_definitions(-DGIT_STATE_UAS="${GIT_STATE_UAS}") 78 | add_definitions(-DGIT_HASH_CATPILOT="${GIT_HASH_CATPILOT}") 79 | add_definitions(-DGIT_STATE_CATPILOT="${GIT_STATE_CATPILOT}") 80 | 81 | set(CMAKE_TOOLCHAIN_FILE catpilot/bsp/mcu/core/${MCU_FAMILY}/${MCU_MODEL}/${MCU_MODEL}_toolchain.cmake) 82 | 83 | set(ASM_FILE ${PROJECT_BINARY_DIR}/${PROJ_NAME}.asm) 84 | 85 | set(MAP_CREATION "ON") 86 | 87 | project(${PROJ_NAME} C ASM) 88 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/firmware) 89 | 90 | message("\n${PROJECT_NAME}:") 91 | message(" Board: ${BOARD}") 92 | message(" MCU family: ${MCU_FAMILY}") 93 | message(" MCU model: ${MCU_MODEL}") 94 | message(" Toolchain: ${CMAKE_TOOLCHAIN_FILE}") 95 | message(" C Compiler: ${CMAKE_C_COMPILER}") 96 | message(" C++ Compiler: ${CMAKE_CXX_COMPILER}") 97 | message(" Compiler flags: ${COMMON_FLAGS}") 98 | message(" Linker flags: ${LINKER_FLAGS}") 99 | message(" Debug port: ${CLI_PORT} ${CLI_BAUDRATE}") 100 | message(" uas-catpilot: ${GIT_HASH_UAS}${GIT_STATE_UAS}") 101 | message(" catpilot: ${GIT_HASH_CATPILOT}${GIT_STATE_CATPILOT}\n") 102 | 103 | add_subdirectory(catpilot) 104 | 105 | include_directories( 106 | ${OS} 107 | ${BSP} 108 | ) 109 | 110 | include(atomics.cmake) 111 | 112 | add_executable(catom-launcher 113 | ${ATOMICS_REG} 114 | # FIXME 115 | catpilot/c-atom/function/conv.c 116 | catpilot/c-atom/function/error.c 117 | ) 118 | 119 | add_executable(${PROJECT_NAME}.elf 120 | main.c 121 | xml_inline.c 122 | xml_inline_cfgs.c 123 | ${ATOMICS_REG} 124 | # FIXME 125 | catpilot/c-atom/function/error.c 126 | catpilot/c-atom/function/conv.c 127 | ) 128 | 129 | target_link_libraries(catom-launcher PUBLIC 130 | c-atom-launcher-static 131 | ${ATOMICS_LIBS} 132 | ) 133 | 134 | target_link_libraries(${PROJECT_NAME}.elf PUBLIC 135 | os-static 136 | bsp-static 137 | c-atom-static 138 | ${ATOMICS_LIBS} 139 | -lm -lnosys 140 | ) 141 | 142 | set(ELF_FILE ${EXECUTABLE_OUTPUT_PATH}/${PROJECT_NAME}.elf) 143 | set(HEX_FILE ${EXECUTABLE_OUTPUT_PATH}/${PROJECT_NAME}.hex) 144 | set(BIN_FILE ${EXECUTABLE_OUTPUT_PATH}/${PROJECT_NAME}.bin) 145 | 146 | add_custom_command(TARGET "${PROJECT_NAME}.elf" POST_BUILD 147 | COMMAND ${CMAKE_OBJCOPY} -Obinary ${ELF_FILE} ${BIN_FILE} 148 | COMMAND ${CMAKE_OBJCOPY} -Oihex ${ELF_FILE} ${HEX_FILE} 149 | ) 150 | 151 | add_custom_command(TARGET "${PROJECT_NAME}.elf" POST_BUILD 152 | COMMAND ${CMAKE_COMMAND} -E copy ${HEX_FILE} "${EXECUTABLE_OUTPUT_PATH}/${PROJECT_NAME}.hex" 153 | COMMAND ${CMAKE_COMMAND} -E copy ${BIN_FILE} "${EXECUTABLE_OUTPUT_PATH}/${PROJECT_NAME}.bin" 154 | COMMAND ${CMAKE_SIZE} -A ${EXECUTABLE_OUTPUT_PATH}/${PROJECT_NAME}.elf 155 | COMMAND ${CMAKE_SIZE} --format=berkeley ${EXECUTABLE_OUTPUT_PATH}/${PROJECT_NAME}.hex 156 | ) 157 | 158 | -------------------------------------------------------------------------------- /config/quad/ibr_ubx.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /config/quad/flow_nav_vel_pos_prop.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | inputs/vel_correction 26 | inputs/vel 27 | 28 | 29 | 30 | inputs/pos_correction 31 | inputs/pos 32 | 33 | 34 | 35 | inputs/q 36 | 37 | 38 | 39 | 9.81 40 | 41 | 42 | 43 | 0 44 | 45 | 46 | 47 | 0.1 48 | 49 | 50 | 51 | zero/output 52 | zero/output 53 | g/output 54 | 55 | 56 | 57 | inputs/q 58 | inputs/a 59 | 60 | 61 | 62 | frame_transformed_a/v/x 63 | 64 | 65 | 66 | frame_transformed_a/v/y 67 | 68 | 69 | 70 | frame_transformed_a/v/z 71 | a_z_bias_quick_fix/output 72 | 73 | 74 | 75 | accel_n/output 76 | accel_e/output 77 | accel_z_biased/output 78 | 79 | 80 | 81 | adjusted_signs/v 82 | g_vector/v 83 | 84 | 85 | 86 | 87 | reset_level_0/output 88 | inputs/init_cmd 89 | 90 | 91 | 92 | vel_compensated/v 93 | accel/v 94 | inputs/vel_init 95 | initial_reg/output 96 | 97 | 98 | 99 | inputs/pos/x 100 | 101 | 102 | 103 | lat_in_radians/output 104 | 105 | 106 | 107 | prop_vel/v/x 108 | 109 | 6378000 110 | 111 | 112 | 113 | dv_lat_in_rad/output 114 | 115 | 116 | 117 | prop_vel/v/y 118 | 119 | 6378000 120 | 121 | 122 | 123 | dv_lon_in_rad/output 124 | lon_correction_factor/output 125 | 126 | 127 | 128 | lon_correction/output 129 | 130 | 131 | 132 | dv_lat_in_rad_deg/output 133 | dv_lon_in_deg/output 134 | prop_vel/v/z 135 | 136 | 137 | 138 | pos_compensated/v 139 | delta_v_vect_conv/v 140 | inputs/pos_init 141 | initial_reg/output 142 | 143 | 144 | 145 | 146 | prop_vel/v 147 | prop_pos/v 148 | 149 | 150 | -------------------------------------------------------------------------------- /config/quad/flow_cont_vel_pos.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 0 30 | 31 | 32 | 33 | 0 34 | 35 | 36 | 37 | inputs/yaw 38 | 39 | 40 | 41 | zero/output 42 | zero/output 43 | yaw_inv/output 44 | 45 | 46 | 47 | inputs/vel 48 | to_body_trn/q 49 | 50 | 51 | 52 | inputs/yaw 53 | zero/output 54 | zero/output 55 | 56 | 57 | 58 | inputs/preset_roll 59 | inputs/preset_pitch 60 | zero/output 61 | 62 | 63 | 64 | pre_set_vector/v 65 | from_body_trn/q 66 | 67 | 68 | 69 | inputs/enable_auto 70 | inputs/enable_vel 71 | 72 | 73 | 74 | inputs/desired_vel_body 75 | to_body_trn/q 76 | 77 | 78 | 79 | desired_vel_ne/v/y 80 | inputs/vel/y 81 | pid_enable_cmd/output 82 | ne_regs_preset/v/x 83 | 84 | 0.15 85 | 0.03 86 | 0 87 | -0.3 88 | 0.3 89 | -0.5 90 | 0.5 91 | 92 | 93 | 94 | desired_vel_ne/v/x 95 | inputs/vel/x 96 | ne_regs_preset/v/y 97 | pid_enable_cmd/output 98 | 99 | 0.15 100 | 0.03 101 | 0 102 | -0.3 103 | 0.3 104 | -0.5 105 | 0.5 106 | 107 | 108 | 109 | pid_enable_cmd/output 110 | inputs/desired_vel_body/z 111 | inputs/vel/z 112 | inputs/preset_collective 113 | 114 | 0.15 115 | 0.07 116 | 0.15 117 | 0.8 118 | 0.15 119 | 1.0 120 | 121 | 122 | 123 | 0 124 | 125 | 126 | 127 | pid_vel_east/output 128 | pid_vel_north/output 129 | zero2/output 130 | 131 | 132 | 133 | to_body_trn/q 134 | cont_vel_sol/v 135 | 136 | 137 | 138 | desired_roll_pitch/v/y 139 | 140 | 141 | 142 | 0 143 | 144 | 145 | 146 | desired_roll_pitch/v/x 147 | desired_pitch/output 148 | desired_yaw_stub/output 149 | 150 | 151 | 152 | 153 | 154 | 155 | pid_vel_vert/output 156 | desired_attitude_concat/v 157 | hold_yaw_cmd/output 158 | pid_enable_cmd/output 159 | inp_vel_frm_transform/v 160 | 161 | 162 | -------------------------------------------------------------------------------- /test_udp_gui.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | from time import sleep 5 | import ctypes 6 | from typing import List 7 | 8 | sys.path.append('./catpilot/c-atom/eswb/pytools') 9 | 10 | from monitor import * 11 | from eswbmon import * 12 | from ds.datasources import * 13 | 14 | from eswb import * 15 | 16 | mon_bus_name = 'monitor' 17 | telemetry_dir_name = 'telemetry' 18 | 19 | args_parser = ArgParser() 20 | args = args_parser.args 21 | 22 | mon = EswbMonitor(monitor_bus_name=mon_bus_name, argv=sys.argv, tabs=True, ) 23 | 24 | main_tab = mon.add_tab('Main') 25 | hk_tab = mon.add_tab('Houskeeping') 26 | sdtl_tab = mon.add_tab('SDTL') 27 | 28 | mon.mkdir(telemetry_dir_name) 29 | 30 | basic_topics_root = mon_bus_name + '/' + telemetry_dir_name 31 | drone_topics_root = basic_topics_root + '/' + 'drone' 32 | 33 | cmd_sdtl_channel = SDTLchannel(name='cmd', ch_id=4, ch_type=SDTLchannelType.unrel) 34 | mon.bridge_sdtl_udp(ip_in='0.0.0.0', port_in='20001', 35 | ip_out='192.168.1.21', port_out='20000', 36 | bridge_to='telemetry', 37 | additional_channels=[cmd_sdtl_channel]) 38 | 39 | ax = DataSourceEswbTopic('ax', path=f'{basic_topics_root}/nav/ang/a/x') 40 | ay = DataSourceEswbTopic('ay', path=f'{basic_topics_root}/nav/ang/a/y') 41 | az = DataSourceEswbTopic('az', path=f'{basic_topics_root}/nav/ang/a/z') 42 | wx = DataSourceEswbTopic('wx', path=f'{basic_topics_root}/nav/ang/omega/x', mult=57.32) 43 | wy = DataSourceEswbTopic('wy', path=f'{basic_topics_root}/nav/ang/omega/y', mult=57.32) 44 | wz = DataSourceEswbTopic('wz', path=f'{basic_topics_root}/nav/ang/omega/z', mult=57.32) 45 | roll = DataSourceEswbTopic('roll', path=f'{basic_topics_root}/nav/ang/roll', mult=57.32) 46 | pitch = DataSourceEswbTopic('pitch', path=f'{basic_topics_root}/nav/ang/pitch', mult=57.32) 47 | yaw = DataSourceEswbTopic('yaw', path=f'{basic_topics_root}/nav/ang/yaw', mult=57.32) 48 | 49 | pstat = DataSourceEswbTopic('pstat', path=f'{basic_topics_root}/nav/pres/pstat') 50 | pdyn = DataSourceEswbTopic('pdyn', path=f'{basic_topics_root}/nav/pres/pdyn') 51 | pdiff = DataSourceEswbTopic('pdiff', path=f'{basic_topics_root}/nav/pres/pdiff') 52 | 53 | altitude_bar = DataSourceEswbTopic('altitude_bar', path=f'{basic_topics_root}/nav/air/altitude_bar') 54 | airspeed = DataSourceEswbTopic('airspeed', path=f'{basic_topics_root}/nav/air/airspeed') 55 | 56 | tax = DataSourceEswbTopic('tax', path=f'{basic_topics_root}/dev/imu/t/ta/x') 57 | tay = DataSourceEswbTopic('tay', path=f'{basic_topics_root}/dev/imu/t/ta/y') 58 | taz = DataSourceEswbTopic('taz', path=f'{basic_topics_root}/dev/imu/t/ta/z') 59 | twx = DataSourceEswbTopic('twx', path=f'{basic_topics_root}/dev/imu/t/tw/x') 60 | twy = DataSourceEswbTopic('twy', path=f'{basic_topics_root}/dev/imu/t/tw/y') 61 | twz = DataSourceEswbTopic('twz', path=f'{basic_topics_root}/dev/imu/t/tw/z') 62 | tadc = DataSourceEswbTopic('tadc', path=f'{basic_topics_root}/dev/imu/t/tadc') 63 | 64 | io_adc1_ch0 = DataSourceEswbTopic('io_adc1_ch0', path=f'{basic_topics_root}/dev/adc/voltage/ch0') 65 | io_adc1_ch1 = DataSourceEswbTopic('io_adc1_ch1', path=f'{basic_topics_root}/dev/adc/voltage/ch1') 66 | io_adc1_ch2 = DataSourceEswbTopic('io_adc1_ch2', path=f'{basic_topics_root}/dev/adc/voltage/ch2') 67 | io_adc1_ch3 = DataSourceEswbTopic('io_adc1_ch3', path=f'{basic_topics_root}/dev/adc/voltage/ch3') 68 | io_adc1_ch4 = DataSourceEswbTopic('io_adc1_ch4', path=f'{basic_topics_root}/dev/adc/voltage/ch4') 69 | io_adc1_ch5 = DataSourceEswbTopic('io_adc1_ch5', path=f'{basic_topics_root}/dev/adc/voltage/ch5') 70 | io_adc1_ch6 = DataSourceEswbTopic('io_adc1_ch6', path=f'{basic_topics_root}/dev/adc/voltage/ch6') 71 | io_adc1_ch7 = DataSourceEswbTopic('io_adc1_ch7', path=f'{basic_topics_root}/dev/adc/voltage/ch7') 72 | io_adc2_ch0 = DataSourceEswbTopic('io_adc2_ch0', path=f'{basic_topics_root}/dev/adc/voltage/ch8') 73 | io_adc2_ch1 = DataSourceEswbTopic('io_adc2_ch1', path=f'{basic_topics_root}/dev/adc/voltage/ch9') 74 | io_adc2_ch2 = DataSourceEswbTopic('io_adc2_ch2', path=f'{basic_topics_root}/dev/adc/voltage/ch10') 75 | io_adc2_ch3 = DataSourceEswbTopic('io_adc2_ch3', path=f'{basic_topics_root}/dev/adc/voltage/ch11') 76 | io_adc2_ch4 = DataSourceEswbTopic('io_adc2_ch4', path=f'{basic_topics_root}/dev/adc/voltage/ch12') 77 | io_adc2_ch5 = DataSourceEswbTopic('io_adc2_ch5', path=f'{basic_topics_root}/dev/adc/voltage/ch13') 78 | io_adc2_ch6 = DataSourceEswbTopic('io_adc2_ch6', path=f'{basic_topics_root}/dev/adc/voltage/ch14') 79 | io_adc2_ch7 = DataSourceEswbTopic('io_adc2_ch7', path=f'{basic_topics_root}/dev/adc/voltage/ch15') 80 | 81 | sin = EwChart([DataSourceEswbTopic('sin1', path=f'{basic_topics_root}/hk/sin1'), 82 | DataSourceEswbTopic('sin2', path=f'{basic_topics_root}/hk/sin2')]) 83 | 84 | accel = EwChart([ax, ay, az], title="Accelerations", 85 | labels={'left': 'm/s²', 'bottom': 'time, s'}) 86 | gyro = EwChart([wx, wy, wz], title="Angular rates", 87 | labels={'left': '1/s', 'bottom': 'time, s'}) 88 | roll_pitch = EwChart([roll, pitch], title="Angles", 89 | labels={'left': 'grad', 'bottom': 'time, s'}) 90 | yaw = EwChart([yaw], title="Angles") 91 | accel_gyro_temp = EwChart([tax, tay, taz, twx, twy, twz], title="Sensor temperatures", 92 | labels={'left': '°C', 'bottom': 'time, s'}) 93 | 94 | pstat_pdyn = EwChart([pstat, pdyn], title="Static and dynamic pressure") 95 | pdiff = EwChart([pdiff], title="Differential pressure") 96 | adc_temp = EwChart([tadc], title="ADC temperature") 97 | 98 | altitude_bar = EwChart([altitude_bar], title="Barometric altitude") 99 | airspeed = EwChart([airspeed], title="Airspeed") 100 | 101 | # io_adc1 = EwChart([io_adc1_ch0, io_adc1_ch1, io_adc1_ch2, io_adc1_ch3, io_adc1_ch4, io_adc1_ch5], title="IO ADC 1 channels") 102 | # io_adc2 = EwChart([io_adc2_ch0, io_adc2_ch1, io_adc2_ch2, io_adc2_ch3, io_adc2_ch4, io_adc2_ch5], title="IO ADC 2 channels") 103 | io_adc1_ch0 = EwChart([io_adc1_ch0], title="IO ADC1 ch0") 104 | io_adc1_ch1 = EwChart([io_adc1_ch1], title="IO ADC1 ch1") 105 | io_adc1_ch2 = EwChart([io_adc1_ch2], title="IO ADC1 ch2") 106 | io_adc1_ch3 = EwChart([io_adc1_ch3], title="IO ADC1 ch3") 107 | io_adc1_ch4 = EwChart([io_adc1_ch4], title="IO ADC1 ch4") 108 | io_adc1_ch5 = EwChart([io_adc1_ch5], title="IO ADC1 ch5") 109 | io_adc1_ch6 = EwChart([io_adc1_ch6], title="IO ADC1 ch6") 110 | io_adc1_ch7 = EwChart([io_adc1_ch7], title="IO ADC1 ch7") 111 | io_adc2_ch0 = EwChart([io_adc2_ch0], title="IO ADC2 ch0") 112 | io_adc2_ch1 = EwChart([io_adc2_ch1], title="IO ADC2 ch1") 113 | io_adc2_ch2 = EwChart([io_adc2_ch2], title="IO ADC2 ch2") 114 | io_adc2_ch3 = EwChart([io_adc2_ch3], title="IO ADC2 ch3") 115 | io_adc2_ch4 = EwChart([io_adc2_ch4], title="IO ADC2 ch4") 116 | io_adc2_ch5 = EwChart([io_adc2_ch5], title="IO ADC2 ch5") 117 | io_adc2_ch6 = EwChart([io_adc2_ch6], title="IO ADC2 ch6") 118 | io_adc2_ch7 = EwChart([io_adc2_ch7], title="IO ADC2 ch7") 119 | 120 | main_tab.add_widget(EwGroup([accel, gyro, roll_pitch, accel_gyro_temp])) 121 | main_tab.add_widget(EwGroup([pstat_pdyn, pdiff, adc_temp, altitude_bar, airspeed])) 122 | main_tab.add_widget(EwGroup([io_adc1_ch0, io_adc1_ch1, io_adc1_ch2, io_adc1_ch3, io_adc1_ch4, io_adc1_ch5, io_adc1_ch6, io_adc1_ch7])) 123 | main_tab.add_widget(EwGroup([io_adc2_ch0, io_adc2_ch1, io_adc2_ch2, io_adc2_ch3, io_adc2_ch4, io_adc2_ch5, io_adc2_ch6, io_adc2_ch7])) 124 | 125 | hk_tab.add_widget(sin) 126 | 127 | sdtl_tab.add_widget(EwGroup([mon.get_stat_widget()])) 128 | 129 | mon.app_window.print_bus_tree() 130 | mon.run() 131 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | CatPilot is a drone's autopilot software stack designed to create scalable, distributed 2 | embedded software systems. 3 | 4 | CatPilot key idea is to use top-level domain-specific notations for specifying desired behavior. And 5 | the minimalistic generalized C-language codebase to execute it for real-time mission-critical applications. 6 | 7 | # Key features 8 | - written in C language; 9 | - extends via atomic functions - reusable blocks with supporting code generation from formal description; 10 | - integrates to the specific vehicle by XML-shaped DSLs, which are orchestrated by model-based design tools; 11 | - provides hardware and OS-agnostic stack that can easily migrate from one hardware to another while growing 12 | from prove-of-concept prototype to the certification grade solution; 13 | - provides services for telemetry transmission, logging and visualization. 14 | 15 | ## Atomic functions 16 | 17 |

18 | 1. Create formal representation 19 | 20 | ```python 21 | 22 | from fspeclib import * 23 | 24 | Function( 25 | name='core.quat.prop', 26 | title=LocalizedString( 27 | en='Propagate quaternion' 28 | ), 29 | inputs=[ 30 | Input( 31 | name='omega', 32 | title='Angular rate vector', 33 | value_type='core.type.v3f64' 34 | ), 35 | 36 | Input( 37 | name='q0', 38 | title='Initial quat', 39 | value_type='core.type.quat' 40 | ), 41 | 42 | Input( 43 | name='q', 44 | title='Recurrent quat', 45 | value_type='core.type.quat' 46 | ), 47 | 48 | Input( 49 | name='reset', 50 | title='Reset', 51 | description='Command for re-initializing output quat by q0', 52 | value_type='core.type.bool', 53 | mandatory=False 54 | ), 55 | ], 56 | outputs=[ 57 | Output( 58 | name='q', 59 | title='Updated quat', 60 | value_type='core.type.quat' 61 | ), 62 | ], 63 | state=[ 64 | Variable( 65 | name='inited', 66 | title='Initialized flag', 67 | value_type='core.type.bool' 68 | ), 69 | ], 70 | 71 | injection=Injection( 72 | timedelta=True 73 | ) 74 | ) 75 | ``` 76 |
77 | 78 |
79 | 2. Generate integration software and the implementation stub 80 | 81 | Simply run: 82 | ```bash 83 | fspecgen.py --code --cmake --f_specs_dirs project:./atomics/ catpilot:catpilot/atomics/ catom:catpilot/c-atom/atomics/ 84 | ``` 85 | Check Manual for details [documentation](https://docs.ctlst.app/catom/atomic-functions.html) for details 86 |
87 | 88 |
89 | 3. Implement behaviour 90 | 91 | ```c 92 | #include "core_quat_prop.h" 93 | 94 | void core_quat_prop_exec( 95 | const core_quat_prop_inputs_t *i, 96 | core_quat_prop_outputs_t *o, 97 | core_quat_prop_state_t *state, 98 | const core_quat_prop_injection_t *injection 99 | ) 100 | { 101 | if (i->optional_inputs_flags.reset) { 102 | if (i->reset) { 103 | state->inited = 0; 104 | } 105 | } 106 | 107 | if (state->inited == FALSE) { 108 | o->q = i->q0; 109 | state->inited = 1; 110 | } else { 111 | o->q.w = i->q.w + -0.5 * ( i->q.x * i->omega.x + i->q.y * i->omega.y + i->q.z * i->omega.z ) * injection->dt; 112 | o->q.x = i->q.x + 0.5 * ( i->q.w * i->omega.x + i->q.y * i->omega.z - i->q.z * i->omega.y ) * injection->dt; 113 | o->q.y = i->q.y + 0.5 * ( i->q.w * i->omega.y + i->q.z * i->omega.x - i->q.x * i->omega.z ) * injection->dt; 114 | o->q.z = i->q.z + 0.5 * ( i->q.w * i->omega.z + i->q.x * i->omega.y - i->q.y * i->omega.x ) * injection->dt; 115 | } 116 | } 117 | 118 | ``` 119 |
120 | 121 |
122 | 4. Integrate and reuse easily 123 | 124 | ```xml 125 | 126 | omega_x/output 127 | zero/output 128 | zero/output 129 | initial_euler/q 130 | norm_att_quat/q 131 | 132 | ``` 133 |
134 | 135 |
136 | 5. Check existing atomic functions catalog 137 | 138 | [Catalog's link](https://docs.ctlst.app/atomics-catalog/catom-atomic-catalog.html) 139 | 140 |
141 | 142 | 143 | ## DSL based integration 144 | 145 | Define top level behavior in the problem-oriented notation: 146 | 147 | - **swsys** - software system description layer; allocates functions and other blocks into tasks and processes. 148 | - **flow** - block to arrange computational graphs as a sequence of atomic reusable functions. 149 | - **fsm** - finite state machine notation, operates by states, transitions and actions on states and transitions. 150 | - **ibr** - interface bridge - designed to take care of converting information from and to other devices. 151 | 152 |
153 | More info 154 | 155 | [Documentation](https://docs.ctlst.app/catom/intro.html) 156 | 157 |
158 | 159 | 160 | ## Model-based design representation 161 | 162 | View this GitHub project in [C-ATLAS](https://s1.ctlst.app/p/STGhxorC) 163 | 164 | ### Target's software system outlook 165 | ![Software system](doc/catlas-swsys.png) 166 | 167 | ### Atomic function orchestration outlook 168 | ![Functional flow](doc/catlas-flow.png) 169 | 170 | ### Hardware integration outlook 171 | ![Hardware schematic](doc/catlas-hw.png) 172 | 173 | ## Quick control interfaces creation 174 | 175 | Check [documentation](https://docs.ctlst.app/uas-catpilot/gui-creation.html) for more info 176 | 177 | ![Vizialization tools](doc/eswbmon.png) 178 | 179 | 180 | # Quick start and documentation 181 | 182 | [Quick start manual](https://docs.ctlst.app/uas-catpilot/quick-start.html) 183 | 184 | [Documentation](https://docs.ctlst.app/uas-catpilot/intro.html) 185 | 186 | # Hardware support 187 | 188 | CatPilot supports: 189 | 1. [CubePilot](https://www.cubepilot.com/) 190 | 2. [Catalyst Aerospace Technologies devices family](https://ctlst.tech/catalog/) 191 | 192 | # Project structure 193 | 194 | ![Software architecture](doc/catpilot-structure.jpg) 195 | 196 | This repository is created to collaborate on functions and configurations 197 | for Unmanned Aerial Systems, while hardware and platform related software is implemented 198 | in [catpilot repo](https://github.com/ctlst-tech/catpilot). CatPilot relates on 199 | [C-ATOM](https://github.com/ctlst-tech/c-atom) library for its generic functionality. 200 | Core platform-agnostic middleware is the 201 | [Embedded Software Bus (ESWB)](https://github.com/ctlst-tech/eswb) library. 202 | 203 | 204 | The project intends to separate knowledge is well-defined related and easily reusable modules. 205 | 206 | # How to contribute 207 | 208 | 1. Join [Discord](https://discord.gg/yn3fm8bjWU), ask questions, raise issues 209 | 2. Create UAS-specific or generic atomic functions 210 | 3. Extend the list of the supported hardware (by the way, C-ATOM is application agnostic embedded stack) 211 | 4. Help to make the documentation crystal clear by adjusting the text or highlighting grey spots and problems 212 | 213 | # Join the community 214 | 215 | [Discord group](https://discord.gg/yn3fm8bjWU) 216 | 217 | [logo_discord.png](https://discord.gg/yn3fm8bjWU) 218 | 219 | # Follow the updates 220 | 221 | - [Twitter](http://twitter.com/ctlst_tech) 222 | - [LinkedIn](https://www.linkedin.com/company/ctlst-tech/) 223 | -------------------------------------------------------------------------------- /config/quad/flow_rc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 0.5 25 | 26 | 27 | 28 | logic_sw_activation_threshold/output 29 | rc/rc_channel5 30 | 31 | 32 | 33 | rc/rc_channel2 34 | 35 | 36 | 37 | rc/rc_channel1 38 | rc_channel2_inv/output 39 | rc/rc_channel4 40 | 41 | 42 | 43 | direct/v/x 44 | 45 | 0.7 46 | 47 | 48 | 49 | direct/v/y 50 | 51 | 0.7 52 | 53 | 54 | 55 | direct/v/z 56 | 57 | 0.7 58 | 59 | 60 | 61 | input_manc_angrate_x/output 62 | input_manc_angrate_y/output 63 | input_manc_angrate_z/output 64 | 65 | 66 | 67 | direct/v/x 68 | 69 | 0.8 70 | 71 | 72 | 73 | direct/v/y 74 | 75 | 0.8 76 | 77 | 78 | 79 | 0 80 | 81 | 82 | 83 | zero/output 84 | input_pitch/output 85 | input_roll/output 86 | 87 | 88 | 89 | rc/rc_channel8 90 | 91 | 1.0 92 | 93 | 94 | 95 | rc/rc_channel7 96 | 97 | 2.0 98 | 99 | 100 | 101 | rc/rc_channel3 102 | 103 | 1.0 104 | 105 | 106 | 107 | 0.0 108 | 109 | 110 | 111 | collective_idle/output 112 | collective_in/output 113 | arm_signal/output 114 | 115 | 116 | 117 | direct/v/y 118 | 119 | 120 | 121 | inv_vel_longit/output 122 | 123 | 2 124 | 125 | 126 | 127 | direct/v/x 128 | 129 | 2 130 | 131 | 132 | 133 | 0.5 134 | 135 | 136 | 137 | rc/rc_channel3 138 | manc_vert_speed_bias/output 139 | 140 | 141 | 142 | manc_vert_speed/output 143 | 144 | 1 145 | 146 | 147 | 148 | input_manc_vel_vert/output 149 | input_manc_vel_transv/output 150 | input_manc_vel_longit/output 151 | 152 | 153 | 154 | rc/rc_channel9 155 | logic_sw_activation_threshold/output 156 | 157 | 158 | 159 | rc/rc_channel6 160 | 161 | 162 | 163 | logic_sw_activation_threshold/output 164 | rc/rc_channel10 165 | 166 | 167 | 168 | enable_direct_control/output 169 | 170 | 171 | 172 | direct_control_disabled/output 173 | ang_vel_mode_switch/mode1 174 | 175 | 176 | 177 | direct_control_disabled/output 178 | ang_vel_mode_switch/mode2 179 | 180 | 181 | 182 | 183 | direct_control_disabled/output 184 | enable_vel/output 185 | enable_ang_pos/output 186 | desired_attutude/v 187 | desired_vel/v 188 | enable_auto_signal/output 189 | desired_omega/v 190 | direct/v 191 | collective_arm_mux/output 192 | pids_att_gain_multiplier/output 193 | pids_rate_gain_multiplier/output 194 | arm_signal/output 195 | 196 | 197 | -------------------------------------------------------------------------------- /config/quad/flow_cont_angrate.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 20 31 | 32 | 33 | 34 | 0.0 35 | 36 | 37 | 38 | inputs/desired_omega/x 39 | inputs/omega/x 40 | omega_integral_preset/output 41 | inputs/enable 42 | 43 | 1.0 44 | 0.03 45 | 0.035 46 | -0.3 47 | 0.3 48 | -1.0 49 | +1.0 50 | 51 | 52 | 53 | inputs/desired_omega/y 54 | inputs/omega/y 55 | omega_integral_preset/output 56 | inputs/enable 57 | 58 | 1.0 59 | 0.03 60 | 0.035 61 | -0.3 62 | 0.3 63 | -1.0 64 | +1.0 65 | 66 | 67 | 68 | inputs/desired_omega/z 69 | inputs/omega/z 70 | omega_integral_preset/output 71 | inputs/enable 72 | 73 | 0.8 74 | 0.1 75 | 0.001 76 | -0.3 77 | 0.3 78 | -1.0 79 | +1.0 80 | 81 | 82 | 83 | 0.0 84 | 85 | 86 | 87 | cont_idle_position/output 88 | cont_idle_position/output 89 | cont_idle_position/output 90 | 91 | 92 | 93 | 0.0 94 | 95 | 96 | 97 | pid_angrate_x/output 98 | inputs/pids_gain 99 | 100 | 101 | 102 | pid_angrate_y/output 103 | inputs/pids_gain 104 | 105 | 106 | 107 | pid_angrate_z/output 108 | inputs/pids_gain 109 | 110 | 111 | 112 | pid_multed_x/output 113 | pid_multed_y/output 114 | pid_multed_z/output 115 | 116 | 117 | 118 | inputs/direct 119 | angrate_cont_auto/v 120 | inputs/enable 121 | 122 | 123 | 124 | cont_idle/v 125 | att_cont_mode_mux/output 126 | inputs/arming 127 | 128 | 129 | 130 | 0.15 131 | 132 | 133 | 134 | inputs/enable_auto_collective 135 | inputs/collective_auto 136 | inputs/collective 137 | 138 | 139 | 140 | collective_auto_mux/output 141 | 142 | 0.85 143 | 144 | 145 | 146 | collective_min/output 147 | collective_input/output 148 | 149 | 150 | 151 | cont_idle_collective/output 152 | collective_sol/output 153 | inputs/enable 154 | 155 | 156 | 157 | att_arm_mux/output/x 158 | att_arm_mux/output/y 159 | collective_arm_mux/output 160 | att_arm_mux/output/z 161 | 162 | -0.15 163 | +0.15 164 | 0.15 165 | -0.15 166 | +0.15 167 | -0.15 168 | 0.15 169 | -0.15 170 | 0.6 171 | +0.2 172 | +0.2 173 | -0.2 174 | -0.2 175 | 176 | 177 | 178 | pwm_probing_meander/output 179 | 180 | 5 181 | 182 | 183 | 184 | inputs/arming 185 | inputs/arming 186 | 187 | 188 | 189 | mixer/m4 190 | mixer/m3 191 | mixer/m2 192 | mixer/m1 193 | arming_logic/output 194 | pwm_probing_meander/output 195 | 196 | 1000 197 | 1000 198 | 1000 199 | 1000 200 | 2000 201 | 2000 202 | 2000 203 | 2000 204 | 205 | 206 | 207 | 208 | att_arm_mux/output 209 | arming_logic/output 210 | mixer/m1 211 | mixer/m2 212 | mixer/m3 213 | mixer/m4 214 | collective_arm_mux/output 215 | 216 | 217 | -------------------------------------------------------------------------------- /gui.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | 5 | sys.path.append("./catpilot/c-atom/eswb/pytools") 6 | 7 | # from controls import * 8 | from monitor import * 9 | from eswbmon import * 10 | 11 | mon_bus_name = 'monitor' 12 | telemetry_dir_name = 'telemetry' 13 | 14 | args_parser = ArgParser() 15 | args = args_parser.args 16 | 17 | mon = EswbMonitor(monitor_bus_name=mon_bus_name, argv=sys.argv, tabs=True) 18 | 19 | front_tab = mon.add_tab('Main') 20 | nav_data_tab = mon.add_tab('Nav Data') 21 | imu_tab = mon.add_tab('IMU') 22 | control_tab = mon.add_tab('Control') 23 | rel_map_tab = mon.add_tab('Rel Map') 24 | map_tab = mon.add_tab('Map') 25 | raw_data_tab = mon.add_tab('Raw data') 26 | sdtl_tab = mon.add_tab('SDTL') 27 | 28 | mon.mkdir(telemetry_dir_name) 29 | 30 | topics_root = mon_bus_name + '/' + telemetry_dir_name 31 | 32 | mon.bridge_sdtl(path=args.serdev, baudrate=args.serbaud, bridge_to='telemetry') 33 | 34 | # att_indicator = EwAttitudeIndicator([DataSourceEswbTopic('roll', path=f'{topics_root}/nav/nav/roll', mult=57.32), 35 | # DataSourceEswbTopic('pitch', path=f'{topics_root}/nav/nav/pitch', mult=57.32)]) 36 | 37 | # 38 | # ai = EwAttitudeIndicator([ 39 | # DataSourceSinus('s1', iphase=0.0, mult=45), 40 | # DataSourceSinus('s2', iphase=1.0, mult=20) 41 | # ]) 42 | 43 | ai = EwAttitudeIndicator([ 44 | DataSourceEswbTopic('roll', path=f'{topics_root}/nav/nav/roll', mult=57.32), 45 | DataSourceEswbTopic('pitch', path=f'{topics_root}/nav/nav/pitch', mult=57.32) 46 | ]) 47 | 48 | ds_yaw = DataSourceEswbTopic('yaw', path=f'{topics_root}/nav/nav/yaw', mult=57.32) 49 | 50 | hi = EwHeadingIndicator([ds_yaw]) 51 | 52 | ds_mag_azimuth = DataSourceEswbTopic('azimuth', path=f'{topics_root}/nav/nav/azimuth', mult=57.32) 53 | 54 | compass = EwHeadingIndicator([ds_mag_azimuth]) 55 | 56 | imu_roll_pitch = EwChart([DataSourceEswbTopic('roll', path=f'{topics_root}/nav/nav/roll', mult=57.32), 57 | DataSourceEswbTopic('pitch', path=f'{topics_root}/nav/nav/pitch', mult=57.32)], 58 | data_range=(-60, +60)) 59 | 60 | imu_omega = EwChart([DataSourceEswbTopic('omega_x', path=f'{topics_root}/nav/nav/omega/x', mult=57.32), 61 | DataSourceEswbTopic('omega_y', path=f'{topics_root}/nav/nav/omega/y', mult=57.32), 62 | DataSourceEswbTopic('omega_z', path=f'{topics_root}/nav/nav/omega/z', mult=57.32)], 63 | data_range=(-60, +60)) 64 | 65 | ds_accel_x = DataSourceEswbTopic('a_x', path=f'{topics_root}/nav/nav/a/x') 66 | ds_accel_y = DataSourceEswbTopic('a_y', path=f'{topics_root}/nav/nav/a/y') 67 | ds_accel_z = DataSourceEswbTopic('a_z', path=f'{topics_root}/nav/nav/a/z') 68 | 69 | imu_a = EwChart([ds_accel_x, 70 | ds_accel_y, 71 | ds_accel_z,], 72 | data_range=(-15, +15)) 73 | 74 | norm_ind_magnitude_ds = DataSourceEswbTopic('mag', path=f'{topics_root}/nav/nav/ind_magnitude') 75 | 76 | ds_magn_x = DataSourceEswbTopic('mag_x', path=f'{topics_root}/nav/nav/induction/x') 77 | ds_magn_y = DataSourceEswbTopic('mag_y', path=f'{topics_root}/nav/nav/induction/y') 78 | ds_magn_z = DataSourceEswbTopic('mag_z', path=f'{topics_root}/nav/nav/induction/z') 79 | 80 | mag = EwChart([ds_magn_x, 81 | ds_magn_y, 82 | ds_magn_z, 83 | norm_ind_magnitude_ds], 84 | data_range=(-0.7, +0.7)) 85 | 86 | hk_sine = EwChart([DataSourceEswbTopic('sine', path=f'{topics_root}/hk/sine'), 87 | # DataSourceSinus('s1', iphase=0.0), 88 | # DataSourceSinus('s2', iphase=1.0) 89 | ], 90 | data_range=(-1, +1)) 91 | 92 | hk_Vbat = EwChart([DataSourceEswbTopic('Vbat', path=f'{topics_root}/hk/Vbat'), 93 | DataSourceEswbTopic('Vbat_min', path=f'{topics_root}/hk/Vbat_min'), 94 | DataSourceEswbTopic('Vbat_max', path=f'{topics_root}/hk/Vbat_max') 95 | ]) 96 | 97 | hk_CurrBat = EwChart([DataSourceEswbTopic('Curr', path=f'{topics_root}/hk/Curr'), 98 | DataSourceEswbTopic('Curr_min', path=f'{topics_root}/hk/Curr_min'), 99 | DataSourceEswbTopic('Curr_max', path=f'{topics_root}/hk/Curr_max') 100 | ]) 101 | 102 | manc_xy = EwCursor([ 103 | (DataSourceEswbTopic('x', path=f'{topics_root}/cont/cont/direct/x'), 104 | DataSourceEswbTopic('y', path=f'{topics_root}/cont/cont/direct/y')), 105 | (DataSourceEswbTopic('x', path=f'{topics_root}/cont/cont/sol/x'), 106 | DataSourceEswbTopic('y', path=f'{topics_root}/cont/cont/sol/y')) 107 | ]) 108 | 109 | ds_gnss_lat = DataSourceEswbTopic(name='gnss_lat', path=f'{topics_root}/nav/nav/gnss_pos/x') 110 | ds_gnss_lon = DataSourceEswbTopic(name='gnss_lon', path=f'{topics_root}/nav/nav/gnss_pos/y') 111 | ds_gnss_alt = DataSourceEswbTopic(name='gnss_alt', path=f'{topics_root}/nav/nav/gnss_pos/z') 112 | 113 | ds_lat = DataSourceEswbTopic(name='lat', path=f'{topics_root}/nav/nav/pos/x') 114 | ds_lon = DataSourceEswbTopic(name='lon', path=f'{topics_root}/nav/nav/pos/y') 115 | ds_alt = DataSourceEswbTopic(name='alt', path=f'{topics_root}/nav/nav/pos/z') 116 | 117 | ds_gnss_vel_n = DataSourceEswbTopic(name='gnss_vel_n', path=f'{topics_root}/nav/nav/gnss_vel/x') 118 | ds_gnss_vel_e = DataSourceEswbTopic(name='gnss_vel_e', path=f'{topics_root}/nav/nav/gnss_vel/y') 119 | ds_gnss_vel_d = DataSourceEswbTopic(name='gnss_vel_d', path=f'{topics_root}/nav/nav/gnss_vel/z') 120 | ds_vel_n = DataSourceEswbTopic(name='vel_n', path=f'{topics_root}/nav/nav/vel/x') 121 | ds_vel_e = DataSourceEswbTopic(name='vel_e', path=f'{topics_root}/nav/nav/vel/y') 122 | ds_vel_d = DataSourceEswbTopic(name='vel_d', path=f'{topics_root}/nav/nav/vel/z') 123 | 124 | vel_body_longit = DataSourceEswbTopic(name='vel_body_longit', path=f'{topics_root}/cont/cont/vel_body/x') 125 | vel_body_transv = DataSourceEswbTopic(name='vel_body_transv', path=f'{topics_root}/cont/cont/vel_body/y') 126 | vel_body_vert = DataSourceEswbTopic(name='vel_body_vert', path=f'{topics_root}/cont/cont/vel_body/z') 127 | 128 | desired_vel_body_longit = DataSourceEswbTopic(name='desired_vel_body_longit', path=f'{topics_root}/cont/cont/desired_vel_body/x') 129 | desired_vel_body_transv = DataSourceEswbTopic(name='desired_vel_body_transv', path=f'{topics_root}/cont/cont/desired_vel_body/y') 130 | desired_vel_body_vert = DataSourceEswbTopic(name='desired_vel_body_vert', path=f'{topics_root}/cont/cont/desired_vel_body/z') 131 | 132 | modes = EwTable(caption='Modes', data_sources=[ 133 | DataSourceEswbTopic(name='armed', path=f'{topics_root}/cont/cont_mode/armed'), 134 | DataSourceEswbTopic(name='enable_angrate', path=f'{topics_root}/cont/cont_mode/enable_angrate'), 135 | DataSourceEswbTopic(name='enable_angpos', path=f'{topics_root}/cont/cont_mode/enable_angpos'), 136 | DataSourceEswbTopic(name='enable_vel', path=f'{topics_root}/cont/cont_mode/enable_vel'), 137 | DataSourceEswbTopic(name='enable_auto', path=f'{topics_root}/cont/cont_mode/enable_auto'), 138 | DataSourceEswbTopic(name='hold_yaw_cmd', path=f'{topics_root}/cont/cont_mode/hold_yaw_cmd'), 139 | DataSourceEswbTopic(name='vel_enabled', path=f'{topics_root}/cont/cont_mode/vel_enabled'), 140 | DataSourceEswbTopic(name='gnss_prec', path=f'{topics_root}/nav/nav/gnss_prec'), 141 | DataSourceEswbTopic(name='gnss_ready', path=f'{topics_root}/nav/nav/gnss_ready'), 142 | ]) 143 | 144 | nav_data_table = EwTable(caption='Nav Data', data_sources=[ 145 | ds_gnss_lat, 146 | ds_gnss_lon, 147 | ds_gnss_alt, 148 | ds_gnss_vel_n, 149 | ds_gnss_vel_e, 150 | ds_gnss_vel_d, 151 | ds_lat, 152 | ds_lon, 153 | ds_alt, 154 | ds_vel_n, 155 | ds_vel_e, 156 | ds_vel_d, 157 | ds_accel_x, 158 | ds_accel_y, 159 | ds_accel_z, 160 | ds_magn_x, 161 | ds_magn_y, 162 | ds_magn_z, 163 | ]) 164 | 165 | gnss_prec = EwChart([DataSourceEswbTopic('gnss_prec', path=f'{topics_root}/nav/nav/gnss_prec')]) 166 | 167 | map = EwLocationMap( 168 | [('gnss', ds_gnss_lat, ds_gnss_lon, ds_mag_azimuth), ('est', ds_lat, ds_lon, ds_yaw)], 169 | with_control=True) 170 | 171 | rel_map = EwRelativePosition([ 172 | DataSourceSinus('plane_phi', mult=360), 173 | DataSourceConst('plane_r', value=60), 174 | DataSourceSinus('plane_course', iphase=0.0, mult=360), 175 | 176 | DataSourceSinus('base_phi', iphase=0.5, mult=360), 177 | DataSourceConst('base_r', value=120), 178 | ]) 179 | 180 | ind_magn_chart = EwChart([norm_ind_magnitude_ds]) 181 | 182 | pos_n_chart = EwChart([ds_gnss_lat, ds_lat]) 183 | pos_e_chart = EwChart([ds_gnss_lon, ds_lon]) 184 | pos_v_chart = EwChart([ds_gnss_alt, ds_alt]) 185 | 186 | vel_n_chart = EwChart([ds_gnss_vel_n, ds_vel_n]) 187 | vel_e_chart = EwChart([ds_gnss_vel_e, ds_vel_e]) 188 | vel_v_chart = EwChart([ds_gnss_vel_d, ds_vel_d]) 189 | 190 | vel_body_chart_longit = EwChart([vel_body_longit, desired_vel_body_longit]) 191 | vel_body_chart_transv = EwChart([vel_body_transv, desired_vel_body_transv]) 192 | vel_body_chart_vert = EwChart([vel_body_vert, desired_vel_body_vert]) 193 | 194 | front_tab.add_widget(EwGroup([ai, hi, compass, manc_xy, modes, mon.get_small_widget()])) 195 | front_tab.add_widget(EwGroup([imu_roll_pitch, gnss_prec, hk_sine])) 196 | front_tab.add_widget(EwGroup([hk_Vbat, hk_CurrBat])) 197 | # front_tab.add_widget(EwGroup([])) 198 | 199 | nav_data_tab.add_widget(nav_data_table) 200 | 201 | imu_tab.add_widget(EwGroup([pos_n_chart, pos_e_chart, pos_v_chart])) 202 | imu_tab.add_widget(EwGroup([vel_n_chart, vel_e_chart, vel_v_chart])) 203 | 204 | imu_tab.add_widget(ind_magn_chart) 205 | 206 | control_tab.add_widget(vel_body_chart_longit) 207 | control_tab.add_widget(vel_body_chart_transv) 208 | control_tab.add_widget(vel_body_chart_vert) 209 | 210 | map_tab.add_widget(map) 211 | 212 | rel_map_tab.add_widget(rel_map) 213 | 214 | raw_data_tab.add_widget(imu_omega) 215 | raw_data_tab.add_widget(imu_a) 216 | raw_data_tab.add_widget(mag) 217 | 218 | sdtl_tab.add_widget(EwGroup([mon.get_stat_widget()])) 219 | mon.run() 220 | -------------------------------------------------------------------------------- /config/quad/swsys.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | nav_bus/$alias 38 | 39 | nav_bus/$alias 40 | 41 | 42 | 43 | 44 | 45 | nav_bus/$alias 46 | nav_bus/imu_reset 47 | 48 | nav_bus/$alias 49 | 50 | 51 | 52 | 53 | 54 | nav_bus/q 55 | 56 | nav_bus/$alias 57 | 58 | 59 | 60 | 61 | /dev/ttyS2 62 | 63 | 64 | nav_bus/gnss/raw/ubx_frame 65 | nav_bus/gnss/raw/rtcm_frame 66 | 67 | 68 | 69 | 70 | 71 | nav_bus/gnss/raw/ubx_frame 72 | 73 | nav_bus/gnss/msg/ 74 | 75 | 76 | 77 | 78 | 79 | nav_bus/gnss/msg/velned/velN 80 | nav_bus/gnss/msg/velned/velE 81 | nav_bus/gnss/msg/velned/velD 82 | nav_bus/gnss/msg/posllh/$alias 83 | nav_bus/gnss/msg/posllh/height 84 | nav_bus/gnss/msg/posllh/hAcc 85 | nav_bus/gnss/msg/posllh/hAcc 86 | 87 | nav_bus/gnss/sol/$alias 88 | 89 | 90 | 91 | 92 | 93 | nav_bus/$alias 94 | nav_bus/gnss/sol/pos 95 | nav_bus/gnss/sol/vel 96 | nav_bus/gnss/sol/ready 97 | nav_bus/$alias 98 | 99 | nav_bus/$alias 100 | 101 | 102 | 103 | 104 | 105 | nav_bus/$alias 106 | nav_bus/gnss/sol/vel 107 | nav_bus/gnss/sol/pos 108 | nav_bus/gnss/sol/ready 109 | 110 | nav_bus/$alias 111 | 112 | 113 | 114 | 115 | 116 | nav_bus/$alias 117 | 118 | nav_bus/$alias 119 | 120 | 121 | 122 | 123 | 124 | control_bus/rc/$alias 125 | control_bus/rc/$alias 126 | control_bus/rc/$alias 127 | control_bus/rc/$alias 128 | 129 | 130 | 131 | 132 | 133 | nav_bus/$alias 134 | control_bus/rc/$alias 135 | nav_bus/roll 136 | nav_bus/pitch 137 | control_bus/rc/direct_collective 138 | 139 | control_bus/auto/$alias 140 | control_bus/auto/$alias 141 | 142 | 143 | 144 | 145 | 146 | nav_bus/$alias 147 | control_bus/rc/desired_attitude 148 | control_bus/auto/desired_attitude 149 | control_bus/rc/desired_omega 150 | control_bus/rc/enable_angpos 151 | control_bus/auto/vel_enabled 152 | control_bus/auto/hold_yaw_cmd 153 | control_bus/rc/pid_gain_angpos 154 | 155 | control_bus/$alias 156 | 157 | 158 | 159 | 160 | 161 | nav_bus/omega 162 | control_bus/desired_omega 163 | control_bus/rc/$alias 164 | control_bus/rc/direct_collective 165 | control_bus/auto/collective 166 | control_bus/rc/pid_gain_angrate 167 | control_bus/rc/enable_angrate 168 | control_bus/auto/vel_enabled 169 | 170 | control_bus/solution/$alias 171 | 172 | 173 | 174 | 175 | 176 | gcu_bus/hk/sine 177 | gcu_bus/hk/Vbat 178 | gcu_bus/hk/Vbat_min 179 | gcu_bus/hk/Vbat_max 180 | gcu_bus/hk/$alias 181 | 182 | 183 | 184 | 185 | 186 | control_bus/rc/direct_collective 187 | tm_bus/odrive/$alias 188 | 189 | 190 | 191 | 192 | nav_bus/$alias 193 | nav_bus/$alias 194 | nav_bus/magnitude 195 | nav_bus/gnss/sol/pos 196 | nav_bus/gnss/sol/vel 197 | nav_bus/pos 198 | nav_bus/vel 199 | nav_bus/gnss/msg/posllh/hAcc 200 | nav_bus/gnss/sol/ready 201 | 202 | 203 | 204 | control_bus/rc/enable_angrate 205 | control_bus/rc/enable_angpos 206 | control_bus/rc/enable_vel 207 | control_bus/rc/enable_auto 208 | control_bus/auto/hold_yaw_cmd 209 | control_bus/auto/vel_enabled 210 | control_bus/solution/armed 211 | 212 | 213 | 214 | control_bus/rc/$alias 215 | control_bus/solution/$alias 216 | control_bus/rc/desired_vel_body 217 | control_bus/auto/vel_body 218 | 219 | 220 | 221 | nav_bus/$alias 222 | nav_bus/magnitude 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | --------------------------------------------------------------------------------