first fully functional demo

.idea/runConfigurations/debug_AFTS.xml

@@ -3,6 +3,8 @@
     <openocd version="1" gdb-port="3333" telnet-port="4444" board-config="$PROJECT_DIR$/openocd.cfg" reset-type="RUN" download-type="ALWAYS">
       <debugger kind="GDB" isBundled="true" />
     </openocd>
-    <method v="2" />
+    <method v="2">
+      <option name="ToolBeforeRunTask" enabled="true" actionId="Tool_External Tools_build afts" />
+    </method>
   </configuration>
 </component>

.idea/runConfigurations/debug_v6c.xml

@@ -3,6 +3,8 @@
     <openocd version="1" gdb-port="3333" telnet-port="4444" board-config="$PROJECT_DIR$/openocd.cfg" reset-type="RUN" download-type="UPDATED_ONLY">
       <debugger kind="GDB" isBundled="true" />
     </openocd>
-    <method v="2" />
+    <method v="2">
+      <option name="ToolBeforeRunTask" enabled="true" actionId="Tool_External Tools_build v6c" />
+    </method>
   </configuration>
 </component>

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_EPH 20
-param set-default EKF2_REQ_EPV 10
+param set-default EKF2_REQ_EPV 20
 # REQUIRED NUMBER OF SATELLITES
 param set-default EKF2_REQ_NSAT 5

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

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

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 SYS_USE_IO 0
-param set-default SENS_IMU_MODE 0
+#param set-default SENS_IMU_MODE 0
 
 # FC INTERFACE (9600 BAUD)
 # MAV_0 - TELEM 1 - TTYS5 - UART7 (PE7/PE8)

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))

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_PX4IO      = 0,
-	PX4_MFT_USB   = 1,
+	PX4_MFT_USB        = 1,
-	PX4_MFT_CAN2  = 2,
+	PX4_MFT_CAN2       = 2,
-	PX4_MFT_CAN3  = 3,
+	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_unknown   = 0,
-	px4_hw_con_onboard  = 1,
+	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
  *

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

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)
+#  if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-	int hw_version_revision = board_get_hw_version();
+	int hw_fmun_id = GET_HW_FMUM_ID();
-#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_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;
 	}

src/modules/fts_control/fts_control.cpp

@@ -36,8 +36,8 @@ FTSController::init()
 
     get_instance()->reset();
 
-	// run every 1s
+	// Task run frequency
-	ScheduleOnInterval(1000_ms);
+	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
+    // Manage communication with GROUND-FTS module and update GROUND-FTS status
-    // and update GROUND-FTS status
-    //TODO manage comm timeout
     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;
 }
 

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<FTSController>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -157,13 +160,15 @@ private:
     uORB::Publication<ground_fts_response_s> _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{};

src/modules/mavlink/streams/MAIN_FTS_STATUS.hpp

@@ -1,5 +1,5 @@
-#ifndef MAIN_FTS_MESSAGE_HPP
+#ifndef MAIN_FTS_STAUS_HPP
-#define MAIN_FTS_MESSAGE_HPP
+#define MAIN_FTS_STATUS_HPP
 
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>

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("<hwtype> [<hwtype2>]",
 			       "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("<hwbase> [<hwbase2>]",
+			       "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();