diff --git a/.idea/runConfigurations/debug_AFTS.xml b/.idea/runConfigurations/debug_AFTS.xml index 241cf53..7082621 100644 --- a/.idea/runConfigurations/debug_AFTS.xml +++ b/.idea/runConfigurations/debug_AFTS.xml @@ -3,6 +3,8 @@ - + + \ No newline at end of file diff --git a/.idea/runConfigurations/debug_v6c.xml b/.idea/runConfigurations/debug_v6c.xml index 82513ea..34d36ae 100644 --- a/.idea/runConfigurations/debug_v6c.xml +++ b/.idea/runConfigurations/debug_v6c.xml @@ -3,6 +3,8 @@ - + + \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts b/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts index d5fa73e..25ad473 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts +++ b/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts @@ -10,6 +10,9 @@ . ${R}etc/init.d/rc.mc_defaults +param set SYS_MC_EST_GROUP 2 +ekf2 start & + param set-default MAV_TYPE 37 # GEOFENCE SETTINGS @@ -24,7 +27,7 @@ param set-default GF_SOURCE 1 ########################## GPS FAILSAFE REQUIREMENTS ###################################### # REQUIRED PRECISION ON GPS POSITION ESTIMATION, METERS -param set-default EKF2_REQ_EPH 10 -param set-default EKF2_REQ_EPV 10 +param set-default EKF2_REQ_EPH 20 +param set-default EKF2_REQ_EPV 20 # REQUIRED NUMBER OF SATELLITES param set-default EKF2_REQ_NSAT 5 diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index f669d83..78d4033 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -28,6 +28,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA238=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_COMMON_TELEMETRY=y +CONFIG_MAVLINK_DIALECT="customNimbus" CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 diff --git a/boards/px4/fts-can/default.px4board b/boards/px4/fts-can/default.px4board index c62728c..b82d97f 100644 --- a/boards/px4/fts-can/default.px4board +++ b/boards/px4/fts-can/default.px4board @@ -13,6 +13,7 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_FTS_CONTROL=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y diff --git a/boards/px4/fts-can/init/rc.board_defaults b/boards/px4/fts-can/init/rc.board_defaults index 6dc2ae8..330c7b6 100644 --- a/boards/px4/fts-can/init/rc.board_defaults +++ b/boards/px4/fts-can/init/rc.board_defaults @@ -13,8 +13,8 @@ param set-default MAV_PROTO_VER 2 param set-default SDLOG_MODE 2 param set-default SDLOG_PROFILE 0 -param set-default SYS_USE_IO 0 -param set-default SENS_IMU_MODE 0 +#param set-default SYS_USE_IO 0 +#param set-default SENS_IMU_MODE 0 # FC INTERFACE (9600 BAUD) # MAV_0 - TELEM 1 - TTYS5 - UART7 (PE7/PE8) diff --git a/boards/px4/fts-can/src/board_config.h b/boards/px4/fts-can/src/board_config.h index d0cf2a3..b4947e3 100644 --- a/boards/px4/fts-can/src/board_config.h +++ b/boards/px4/fts-can/src/board_config.h @@ -146,7 +146,7 @@ #define V6C21 HW_VER_REV(0x2,0x1) // FMUV6CMini, Rev 1 I2C4 Internal I2C2 External /*FTS*/ -#define GPIO_FTS_ARM /* PE0 FTS CAN ARM NEGATED */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN0) +#define GPIO_FTS_ARM /* PE0 FTS CAN ARM NEGATED */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN0) #define GPIO_FTS_CAN_OUTPUT /* PE2 FTS CAN CUT */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN2) #define GPIO_FTS_CHUTE_OUTPUT /* PE4 FTS CHUTE FIRE */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) #define FTS_ARM_NEGATED(on_true) px4_arch_gpiowrite(GPIO_FTS_ARM, (on_true)) diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 7a6af77..8e7e096 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -265,6 +265,18 @@ # define HW_VER_REV(v,r) ((uint32_t)((v) & 0xffff) << 16) | ((uint32_t)(r) & 0xffff) #endif +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +typedef uint16_t hw_fmun_id_t; +typedef uint16_t hw_base_id_t; +// Original Signals GPIO_HW_REV_SENSE/GPIO_HW_VER_REV_DRIVE is used to ID the FMUM +// Original Signals GPIO_HW_VER_SENSE/GPIO_HW_VER_REV_DRIVE is used to ID the BASE +# define BOARD_HAS_VERSIONING 1 +# define HW_FMUM_ID(rev) ((hw_fmun_id_t)(rev) & 0xffff) +# define HW_BASE_ID(ver) ((hw_base_id_t)(ver) & 0xffff) +# define GET_HW_FMUM_ID() (HW_FMUM_ID(board_get_hw_revision())) +# define GET_HW_BASE_ID() (HW_BASE_ID(board_get_hw_version())) +#endif + #define HW_INFO_REV_DIGITS 3 #define HW_INFO_VER_DIGITS 3 @@ -654,20 +666,51 @@ bool board_booted_by_px4(void); ************************************************************************************/ typedef enum { - PX4_MFT_PX4IO = 0, - PX4_MFT_USB = 1, - PX4_MFT_CAN2 = 2, - PX4_MFT_CAN3 = 3, + PX4_MFT_PX4IO = 0, + PX4_MFT_USB = 1, + PX4_MFT_CAN2 = 2, + PX4_MFT_CAN3 = 3, + PX4_MFT_PM2 = 4, + PX4_MFT_ETHERNET = 5, + PX4_MFT_T1_ETH = 6, + PX4_MFT_T100_ETH = 7, + PX4_MFT_T1000_ETH = 8, } px4_hw_mft_item_id_t; +typedef int (*system_query_func_t)(const char *sub, const char *val, void *out); + +#define PX4_MFT_MFT_TYPES { \ + PX4_MFT_PX4IO, \ + PX4_MFT_USB, \ + PX4_MFT_CAN2, \ + PX4_MFT_CAN3, \ + PX4_MFT_PM2, \ + PX4_MFT_ETHERNET, \ + PX4_MFT_T1_ETH, \ + PX4_MFT_T100_ETH, \ + PX4_MFT_T1000_ETH } + +#define PX4_MFT_MFT_STR_TYPES { \ + "MFT_PX4IO", \ + "MFT_USB", \ + "MFT_CAN2", \ + "MFT_CAN3", \ + "MFT_PM2", \ + "MFT_ETHERNET", \ + "MFT_T1_ETH", \ + "MFT_T100_ETH", \ + "MFT_T1000_ETH", \ + "MFT_T1000_ETH"} + typedef enum { - px4_hw_con_unknown = 0, - px4_hw_con_onboard = 1, + px4_hw_con_unknown = 0, + px4_hw_con_onboard = 1, px4_hw_con_connector = 3, } px4_hw_connection_t; typedef struct { + unsigned int id: 16; /* The id px4_hw_mft_item_id_t */ unsigned int present: 1; /* 1 if this board have this item */ unsigned int mandatory: 1; /* 1 if this item has to be present and working */ unsigned int connection: 2; /* See px4_hw_connection_t */ @@ -679,7 +722,7 @@ typedef const px4_hw_mft_item_t *px4_hw_mft_item; #if defined(BOARD_HAS_VERSIONING) __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id); - +__EXPORT int system_query_manifest(const char *sub, const char *val, void *out); # define PX4_MFT_HW_SUPPORTED(ID) (board_query_manifest((ID))->present) # define PX4_MFT_HW_REQUIRED(ID) (board_query_manifest((ID))->mandatory) # define PX4_MFT_HW_IS_ONBOARD(ID) (board_query_manifest((ID))->connection == px4_hw_con_onboard) @@ -752,6 +795,26 @@ __EXPORT const char *board_get_hw_type_name(void); #define board_get_hw_type_name() "" #endif +/************************************************************************************ + * Name: board_get_hw_base_type_name + * + * Description: + * Optional returns a 0 terminated string defining the HW type. + * + * Input Parameters: + * None + * + * Returned Value: + * a 0 terminated string defining the HW type. This may be a 0 length string "" + * + ************************************************************************************/ + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +__EXPORT const char *board_get_hw_base_type_name(void); +#else +#define board_get_hw_base_type_name() "" +#endif + /************************************************************************************ * Name: board_get_hw_version * diff --git a/platforms/common/include/px4_platform_common/spi.h b/platforms/common/include/px4_platform_common/spi.h index 6664409..a2e4682 100644 --- a/platforms/common/include/px4_platform_common/spi.h +++ b/platforms/common/include/px4_platform_common/spi.h @@ -73,7 +73,11 @@ struct px4_spi_bus_t { struct px4_spi_bus_all_hw_t { px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS]; - int board_hw_version_revision{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + hw_fmun_id_t board_hw_fmun_id {USHRT_MAX}; +#else + int board_hw_version_revision {-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused +#endif }; #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp index 62713b0..6a29a10 100644 --- a/platforms/common/spi.cpp +++ b/platforms/common/spi.cpp @@ -44,12 +44,26 @@ #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 void px4_set_spi_buses_from_hw_version() { -#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) - int hw_version_revision = board_get_hw_version(); -#else - int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision()); -#endif +# if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + int hw_fmun_id = GET_HW_FMUM_ID(); + for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) { + if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_fmun_id == 0) { + px4_spi_buses = px4_spi_buses_all_hw[i].buses; + } + + if (px4_spi_buses_all_hw[i].board_hw_fmun_id == hw_fmun_id) { + px4_spi_buses = px4_spi_buses_all_hw[i].buses; + } + } + +# else + +# if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) + int hw_version_revision = board_get_hw_version(); +# else + int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision()); +# endif for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) { if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version_revision == 0) { @@ -61,6 +75,8 @@ void px4_set_spi_buses_from_hw_version() } } +# endif + if (!px4_spi_buses) { // fallback px4_spi_buses = px4_spi_buses_all_hw[0].buses; } diff --git a/src/modules/fts_control/fts_control.cpp b/src/modules/fts_control/fts_control.cpp index 0645004..43330e3 100644 --- a/src/modules/fts_control/fts_control.cpp +++ b/src/modules/fts_control/fts_control.cpp @@ -36,8 +36,8 @@ FTSController::init() get_instance()->reset(); - // run every 1s - ScheduleOnInterval(1000_ms); + // Task run frequency + ScheduleOnInterval(500_ms); return true; } @@ -56,6 +56,7 @@ FTSController::Run() { //TODO manage comm timeout if (_auto_fts_message_sub.updated()) { _auto_fts_message_sub.copy(&_auto_fts_message); + _auto_fts_timestamp_last = _auto_fts_message.timestamp; //ARMING CONDITIONS ARE MET switch (_auto_fts_state){ case AUTO_FTS_STATE_ARMED:{ @@ -100,12 +101,14 @@ FTSController::Run() { //DO NOTHING }break; } + } else{ + if (hrt_elapsed_time(&_auto_fts_timestamp_last) > AUTO_FTS_COMM_TIMEOUT){ + _auto_fts_state = AUTO_FTS_STATE_DISARMED; + } } - // Manage communication with GROUND-FTS module - // and update GROUND-FTS status - //TODO manage comm timeout + // Manage communication with GROUND-FTS module and update GROUND-FTS status if (_ground_fts_message_sub.updated()) { _ground_fts_message_sub.copy(&_ground_fts_message); @@ -119,6 +122,7 @@ FTSController::Run() { _ground_fts_response.fts_uid = fts_id.uid; _ground_fts_response_pub.publish(_ground_fts_response); + _ground_fts_timestamp_last = _ground_fts_message.timestamp; } } else if (_ground_fts_message.message == PAIR) { //PAIR COMMAND @@ -127,6 +131,10 @@ FTSController::Run() { _ground_fts_response_pub.publish(_ground_fts_response); } + } else { + if (hrt_elapsed_time(&_ground_fts_timestamp_last) > GROUND_FTS_COMM_TIMEOUT){ + _ground_fts_state = GROUND_FTS_STATE_DISARMED; + } } if (_emergency_command_sub.updated()) { @@ -168,14 +176,21 @@ FTSController::Run() { switch (_main_fts_state) { case MAIN_FTS_STATE_DISARMED:{ + if (_ground_fts_state == GROUND_FTS_STATE_FIRE || _auto_fts_state ==AUTO_FTS_STATE_FIRE){ _main_fts_state = MAIN_FTS_STATE_ERROR; } + else if (_ground_fts_state == GROUND_FTS_STATE_ARMED || _auto_fts_state ==AUTO_FTS_STATE_ARMED){ + get_instance()->arm(); + } + }break; case MAIN_FTS_STATE_ARMED:{ if (_ground_fts_state == GROUND_FTS_STATE_FIRE || _auto_fts_state ==AUTO_FTS_STATE_FIRE){ get_instance()->fire(); + } else if (_ground_fts_state == GROUND_FTS_STATE_DISARMED && _auto_fts_state ==AUTO_FTS_STATE_DISARMED){ + get_instance()->disarm(); } }break; case MAIN_FTS_STATE_FIRE: @@ -250,11 +265,11 @@ FTSController::fire() uint8_t FTSController::reset() { - _main_fts_state = MAIN_FTS_STATE_ARMED; + _main_fts_state = MAIN_FTS_STATE_DISARMED; _ground_fts_state = GROUND_FTS_STATE_DISARMED; _auto_fts_state = AUTO_FTS_STATE_DISARMED; fts_drv.fts_off(); - fts_drv.fts_arm(); + fts_drv.fts_disarm(); return 0; } @@ -263,8 +278,6 @@ FTSController::arm() { fts_drv.fts_arm(); _main_fts_state = MAIN_FTS_STATE_ARMED; - _ground_fts_state = GROUND_FTS_STATE_DISARMED; - _auto_fts_state = AUTO_FTS_STATE_DISARMED; return 0; } diff --git a/src/modules/fts_control/fts_control.hpp b/src/modules/fts_control/fts_control.hpp index e02e11e..e961119 100644 --- a/src/modules/fts_control/fts_control.hpp +++ b/src/modules/fts_control/fts_control.hpp @@ -74,6 +74,9 @@ typedef enum AUTO_FTS_STATE_ENUM AUTO_FTS_STATE_FIRE=3, } AUTO_FTS_STATE; +#define GROUND_FTS_COMM_TIMEOUT 5000000_us +#define AUTO_FTS_COMM_TIMEOUT 5000000_us + class FTSController : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -157,13 +160,15 @@ private: uORB::Publication _ground_fts_response_pub{ORB_ID(ground_fts_response)}; FTS fts_drv; - hrt_abstime _last_fts_state_update_time{0}; // Internal module status MAIN_FTS_STATE _main_fts_state; GROUND_FTS_STATE _ground_fts_state; AUTO_FTS_STATE _auto_fts_state; + hrt_abstime _last_fts_state_update_time{0}; + hrt_abstime _ground_fts_timestamp_last{0}; + hrt_abstime _auto_fts_timestamp_last{0}; // UORB topics published main_fts_status_s _main_fts_status_msg{}; diff --git a/src/modules/mavlink/streams/MAIN_FTS_STATUS.hpp b/src/modules/mavlink/streams/MAIN_FTS_STATUS.hpp index d622a2d..74c7119 100644 --- a/src/modules/mavlink/streams/MAIN_FTS_STATUS.hpp +++ b/src/modules/mavlink/streams/MAIN_FTS_STATUS.hpp @@ -1,5 +1,5 @@ -#ifndef MAIN_FTS_MESSAGE_HPP -#define MAIN_FTS_MESSAGE_HPP +#ifndef MAIN_FTS_STAUS_HPP +#define MAIN_FTS_STATUS_HPP #include #include diff --git a/src/systemcmds/ver/ver.cpp b/src/systemcmds/ver/ver.cpp index ab75009..a2b5c18 100644 --- a/src/systemcmds/ver/ver.cpp +++ b/src/systemcmds/ver/ver.cpp @@ -51,6 +51,9 @@ static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[] = "hwcmp"; static const char sz_ver_hwtypecmp_str[] = "hwtypecmp"; +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static const char sz_ver_hwbasecmp_str[] = "hwbasecmp"; +#endif static const char sz_ver_git_str[] = "git"; static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_buri_str[] = "uri"; @@ -84,6 +87,11 @@ static void usage(const char *reason) PRINT_MODULE_USAGE_COMMAND_DESCR("hwtypecmp", "Compare hardware type (returns 0 on match)"); PRINT_MODULE_USAGE_ARG(" []", "Hardware type to compare against (eg. V2). An OR comparison is used if multiple are specified", false); +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + PRINT_MODULE_USAGE_COMMAND_DESCR("hwbasecmp", "Compare hardware base (returns 0 on match)"); + PRINT_MODULE_USAGE_ARG(" []", + "Hardware type to compare against (eg. V2). An OR comparison is used if multiple are specified", false); +#endif } extern "C" __EXPORT int ver_main(int argc, char *argv[]) @@ -129,12 +137,50 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[]) return 1; } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + + if (!strncmp(argv[1], sz_ver_hwbasecmp_str, sizeof(sz_ver_hwbasecmp_str))) { + if (argc >= 3 && argv[2] != nullptr) { + const char *board_type = px4_board_base_type(); + + for (int i = 2; i < argc; ++i) { + if (strcmp(board_type, argv[i]) == 0) { + return 0; // if one of the arguments match, return success + } + } + + } else { + PX4_ERR("Not enough arguments, try 'ver hwbasecmp {000...999}[1:*]'"); + } + + return 1; + } + +#endif /* check if we want to show all */ bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)); if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { PX4_INFO_RAW("HW arch: %s\n", px4_board_name()); -#if defined(BOARD_HAS_VERSIONING) + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + char sbase[14] = "NA"; + char sfmum[14] = "NA"; + int base = GET_HW_BASE_ID(); + int fmu = GET_HW_FMUM_ID(); + + if (base >= 0) { + snprintf(sbase, sizeof(sbase), "0x%0" STRINGIFY(HW_INFO_VER_DIGITS) "X", base); + } + + if (fmu >= 0) { + snprintf(sfmum, sizeof(sfmum), "0x%0" STRINGIFY(HW_INFO_REV_DIGITS) "X", fmu); + } + + PX4_INFO_RAW("HW type: %s\n", strlen(HW_INFO_INIT_PREFIX) ? HW_INFO_INIT_PREFIX : "NA"); + PX4_INFO_RAW("HW FMUM ID: %s\n", sfmum); + PX4_INFO_RAW("HW BASE ID: %s\n", sbase); +#elif defined(BOARD_HAS_VERSIONING) char vb[14] = "NA"; char rb[14] = "NA"; int v = px4_board_hw_version();