WIP: parameter export hacks

Signed-off-by: Alex Mikhalev <alex@corvus-robotics.com>

.ci/Jenkinsfile-hardware

@@ -1030,6 +1030,7 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gps"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag"'
@@ -1041,6 +1042,7 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_acceleration"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_gps_position"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position"'

.vscode/settings.json

@@ -137,5 +137,8 @@
         "workbench.settings.enableNaturalLanguageSearch": false,
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
-        }
+        },
+        "python.autoComplete.extraPaths": [
+                "/opt/ros/foxy/lib/python3.8/site-packages"
+        ]
 }

README.md

@@ -17,7 +17,7 @@ PX4 is highly portable, OS-independent and supports Linux, NuttX and QuRT out of
   * [VTOL](https://docs.px4.io/master/en/frames_vtol/)
   * [Autogyro](https://docs.px4.io/master/en/frames_autogyro/)
   * [Rover](https://docs.px4.io/master/en/frames_rover/)
-  * many more experimental types (Blimps, Boats, Submarines, Autogyros, etc)
+  * many more experimental types (Blimps, Boats, Submarines, High altitude balloons, etc)
 * Releases: [Downloads](https://github.com/PX4/PX4-Autopilot/releases)
 
 

boards/cubepilot/cubeorange/console.cmake

@@ -43,6 +43,7 @@ px4_add_board(
 		#pca9685_pwm_out
 		power_monitor/ina226
 		#protocol_splitter
+		pwm_input
 		pwm_out_sim
 		pwm_out
 		px4io

boards/cubepilot/cubeorange/default.cmake

@@ -44,6 +44,7 @@ px4_add_board(
 		pca9685_pwm_out
 		power_monitor/ina226
 		#protocol_splitter
+		pwm_input
 		pwm_out_sim
 		pwm_out
 		px4io

boards/cubepilot/cubeorange/src/board_config.h

@@ -115,6 +115,11 @@
 #define GPIO_TONE_ALARM_IDLE    GPIO_BUZZER_1
 #define GPIO_TONE_ALARM         GPIO_TIM2_CH1OUT_2
 
+/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
+#define PWMIN_TIMER                       4
+#define PWMIN_TIMER_CHANNEL    /* T4C2 */ 2
+#define GPIO_PWM_IN            /* PD13 */ GPIO_TIM4_CH2IN_2
+
 /* USB
  *  OTG FS: PA9  OTG_FS_VBUS VBUS sensing
  */

msg/CMakeLists.txt

@@ -148,8 +148,8 @@ set(msg_files
 	trajectory_waypoint.msg
 	transponder_report.msg
 	tune_control.msg
-	parameter_request.msg
-	parameter_value.msg
+	uavcan_parameter_request.msg
+	uavcan_parameter_value.msg
 	ulog_stream.msg
 	ulog_stream_ack.msg
 	vehicle_acceleration.msg

msg/parameter_request.msg

@@ -1,26 +0,0 @@
-# UAVCAN-MAVLink parameter bridge request type
-uint64 timestamp                                # time since system start (microseconds)
-
-uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 20      # MAVLINK_MSG_ID_PARAM_REQUEST_READ
-uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 21      # MAVLINK_MSG_ID_PARAM_REQUEST_LIST
-uint8 MESSAGE_TYPE_PARAM_SET          = 23      # MAVLINK_MSG_ID_PARAM_SET
-uint8 message_type                              # message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET
-
-uint8 NODE_ID_ALL = 0           # MAV_COMP_ID_ALL
-uint8 node_id                   # UAVCAN node ID mapped from MAVLink component ID
-
-char[17] param_id               # parameter name
-int16 param_index               # -1 if the param_id field should be used as identifier
-
-uint8 TYPE_UINT8  = 1
-uint8 TYPE_INT32  = 6
-uint8 TYPE_INT64  = 8
-uint8 TYPE_REAL32 = 9
-uint8 param_type                # parameter type
-
-int64 int_value                 # current value if param_type is int-like
-float32 real_value              # current value if param_type is float-like
-
-uint8 ORB_QUEUE_LENGTH = 4
-
-# TOPICS parameter_request uavcan_parameter_request

msg/sensor_gps.msg

@@ -2,6 +2,8 @@
 # the field 'timestamp' is for the position & velocity (microseconds)
 uint64 timestamp		# time since system start (microseconds)
 
+uint32 device_id                # unique device ID for the sensor that does not change between power cycles
+
 int32 lat			# Latitude in 1E-7 degrees
 int32 lon			# Longitude in 1E-7 degrees
 int32 alt			# Altitude in 1E-3 meters above MSL, (millimetres)

msg/uavcan_parameter_request.msg

@@ -0,0 +1,23 @@
+# UAVCAN-MAVLink parameter bridge request type
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 20	# MAVLINK_MSG_ID_PARAM_REQUEST_READ
+uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 21	# MAVLINK_MSG_ID_PARAM_REQUEST_LIST
+uint8 MESSAGE_TYPE_PARAM_SET = 23		# MAVLINK_MSG_ID_PARAM_SET
+uint8 message_type				# MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET
+
+uint8 NODE_ID_ALL = 0		# MAV_COMP_ID_ALL
+uint8 node_id			# UAVCAN node ID mapped from MAVLink component ID
+
+char[17] param_id		# MAVLink/UAVCAN parameter name
+int16 param_index		# -1 if the param_id field should be used as identifier
+
+uint8 PARAM_TYPE_UINT8 = 1	# MAV_PARAM_TYPE_UINT8
+uint8 PARAM_TYPE_INT64 = 8	# MAV_PARAM_TYPE_INT64
+uint8 PARAM_TYPE_REAL32 = 9	# MAV_PARAM_TYPE_REAL32
+uint8 param_type		# MAVLink parameter type
+
+int64 int_value			# current value if param_type is int-like
+float32 real_value		# current value if param_type is float-like
+
+uint8 ORB_QUEUE_LENGTH = 4

msg/uavcan_parameter_value.msg

@@ -1,17 +1,9 @@
 # UAVCAN-MAVLink parameter bridge response type
 uint64 timestamp		# time since system start (microseconds)
-
 uint8 node_id			# UAVCAN node ID mapped from MAVLink component ID
-
 char[17] param_id		# MAVLink/UAVCAN parameter name
-
 int16 param_index		# parameter index, if known
-
 uint16 param_count		# number of parameters exposed by the node
-
-uint8 param_type                # parameter type
-
-int64 int_value                 # current value if param_type is int-like
-float32 real_value              # current value if param_type is float-like
-
-# TOPICS parameter_value uavcan_parameter_value
+uint8 param_type		# MAVLink parameter type
+int64 int_value			# current value if param_type is int-like
+float32 real_value		# current value if param_type is float-like

platforms/nuttx/src/px4/common/board_crashdump.c

@@ -369,7 +369,7 @@ __EXPORT void board_crashdump(uintptr_t currentsp, FAR void *tcb, FAR const char
 		pdump->info.flags |= eInvalidUserStackPtr;
 	}
 
-	int rv = px4_savepanic(HARDFAULT_FILENO, (uint8_t *)pdump, sizeof(fullcontext_s));
+	int rv = stm32_bbsram_savepanic(HARDFAULT_FILENO, (uint8_t *)pdump, sizeof(fullcontext_s));
 
 	/* Test if memory got wiped because of using _sdata */
 

platforms/nuttx/src/px4/nxp/k66/include/px4_arch/micro_hal.h

@@ -87,10 +87,6 @@ __BEGIN_DECLS
 #define PX4_CPU_UUID_WORD32_FORMAT_SIZE         (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1)
 #define PX4_CPU_MFGUID_FORMAT_SIZE              ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1)
 
-#define kinetis_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet
-
-#define px4_savepanic(fileno, context, length)   kinetis_bbsram_savepanic(fileno, context, length)
-
 /* bus_num is zero based on kinetis and must be translated from the legacy one based */
 
 #define PX4_BUS_OFFSET       1                  /* Kinetis buses are 0 based and adjustment is needed */

platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h

@@ -82,10 +82,6 @@ __BEGIN_DECLS
 #define PX4_CPU_UUID_WORD32_FORMAT_SIZE         (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1)
 #define PX4_CPU_MFGUID_FORMAT_SIZE              ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1)
 
-#define imxrt_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet
-
-#define px4_savepanic(fileno, context, length)   imxrt_bbsram_savepanic(fileno, context, length)
-
 /* bus_num is 1 based on imx and must be translated from the legacy one based */
 
 #define PX4_BUS_OFFSET       0                  /* imxrt buses are 1 based no adjustment needed */

platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h

@@ -89,10 +89,6 @@ __BEGIN_DECLS
 #define PX4_CPU_UUID_WORD32_FORMAT_SIZE         (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1)
 #define PX4_CPU_MFGUID_FORMAT_SIZE              ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1)
 
-#define s32k1xx_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet
-
-#define px4_savepanic(fileno, context, length)   s32k1xx_bbsram_savepanic(fileno, context, length)
-
 /* bus_num is zero based on s32k1xx and must be translated from the legacy one based */
 
 #define PX4_BUS_OFFSET       1                  /* s32k1xx buses are 0 based and adjustment is needed */

platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/micro_hal.h

@@ -88,8 +88,6 @@ __BEGIN_DECLS
 #define PX4_CPU_UUID_WORD32_FORMAT_SIZE         (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1)
 #define PX4_CPU_MFGUID_FORMAT_SIZE              ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1)
 
-#define px4_savepanic(fileno, context, length)  stm32_bbsram_savepanic(fileno, context, length)
-
 #define PX4_BUS_OFFSET       0                  /* STM buses are 1 based no adjustment needed */
 #define px4_spibus_initialize(bus_num_1based)   stm32_spibus_initialize(bus_num_1based)
 

src/drivers/drv_sensor.h

@@ -177,6 +177,22 @@
 #define DRV_DIST_DEVTYPE_SIM 0x9a
 #define DRV_DIST_DEVTYPE_SRF05 0x9b
 
+
+#define DRV_GPS_DEVTYPE_ASHTECH 0xA0
+#define DRV_GPS_DEVTYPE_EMLID_REACH 0xA1
+#define DRV_GPS_DEVTYPE_FEMTOMES 0xA2
+#define DRV_GPS_DEVTYPE_MTK 0xA3
+#define DRV_GPS_DEVTYPE_SBF 0xA4
+#define DRV_GPS_DEVTYPE_UBX     0xA5
+#define DRV_GPS_DEVTYPE_UBX_6   0xA6
+#define DRV_GPS_DEVTYPE_UBX_7   0xA7
+#define DRV_GPS_DEVTYPE_UBX_8   0xA8
+#define DRV_GPS_DEVTYPE_UBX_9   0xA9
+#define DRV_GPS_DEVTYPE_UBX_F9P 0xAA
+
+#define DRV_GPS_DEVTYPE_SIM 0xAF
+
+
 #define DRV_DEVTYPE_UNUSED		0xff
 
 #endif /* _DRV_SENSOR_H */

src/drivers/gps/gps.cpp

@@ -47,6 +47,8 @@
 
 #include <termios.h>
 
+#include <drivers/drv_sensor.h>
+#include <lib/drivers/device/Device.hpp>
 #include <lib/parameters/param.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
@@ -91,7 +93,7 @@ struct GPS_Sat_Info {
 static constexpr int TASK_STACK_SIZE = 1760;
 
 
-class GPS : public ModuleBase<GPS>
+class GPS : public ModuleBase<GPS>, public device::Device
 {
 public:
 
@@ -260,6 +262,7 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]);
 
 GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info,
 	 Instance instance, unsigned configured_baudrate) :
+	Device(MODULE_NAME),
 	_configured_baudrate(configured_baudrate),
 	_mode(mode),
 	_interface(interface),
@@ -280,6 +283,16 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
 	}
 
+	if (_interface == GPSHelper::Interface::UART) {
+		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL);
+
+		char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
+		set_device_bus(atoi(&c));
+
+	} else if (_interface == GPSHelper::Interface::SPI) {
+		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
+	}
+
 	if (_mode == GPS_DRIVER_MODE_NONE) {
 		// use parameter to select mode if not provided via CLI
 		char protocol_param_name[16];
@@ -345,7 +358,7 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 	case GPSCallbackType::writeDeviceData:
 		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true);
 
-		return write(gps->_serial_fd, data1, (size_t)data2);
+		return ::write(gps->_serial_fd, data1, (size_t)data2);
 
 	case GPSCallbackType::setBaudrate:
 		return gps->setBaudrate(data2);
@@ -417,7 +430,7 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 #ifdef __PX4_NUTTX
 			int err = 0;
 			int bytes_available = 0;
-			err = ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
+			err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
 
 			if (err != 0 || bytes_available < (int)character_count) {
 				px4_usleep(sleeptime);
@@ -662,14 +675,14 @@ GPS::run()
 
 	if (_interface == GPSHelper::Interface::SPI) {
 		int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
-		int status_value = ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
+		int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
 
 		if (status_value < 0) {
 			PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
 			return;
 		}
 
-		status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
+		status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
 
 		if (status_value < 0) {
 			PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
@@ -743,19 +756,23 @@ GPS::run()
 		case GPS_DRIVER_MODE_UBX:
 			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
 						   gps_ubx_dynmodel, heading_offset, ubx_mode);
+			set_device_type(DRV_GPS_DEVTYPE_UBX);
 			break;
 #ifndef CONSTRAINED_FLASH
 
 		case GPS_DRIVER_MODE_MTK:
 			_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
+			set_device_type(DRV_GPS_DEVTYPE_MTK);
 			break;
 
 		case GPS_DRIVER_MODE_ASHTECH:
 			_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
+			set_device_type(DRV_GPS_DEVTYPE_ASHTECH);
 			break;
 
 		case GPS_DRIVER_MODE_EMLIDREACH:
 			_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
+			set_device_type(DRV_GPS_DEVTYPE_EMLID_REACH);
 			break;
 #endif // CONSTRAINED_FLASH
 
@@ -779,6 +796,37 @@ GPS::run()
 
 				/* GPS is obviously detected successfully, reset statistics */
 				_helper->resetUpdateRates();
+
+				// populate specific ublox model
+				if (get_device_type() == DRV_GPS_DEVTYPE_UBX) {
+					GPSDriverUBX *driver_ubx = (GPSDriverUBX *)_helper;
+
+					switch (driver_ubx->board()) {
+					case GPSDriverUBX::Board::u_blox6:
+						set_device_type(DRV_GPS_DEVTYPE_UBX_6);
+						break;
+
+					case GPSDriverUBX::Board::u_blox7:
+						set_device_type(DRV_GPS_DEVTYPE_UBX_7);
+						break;
+
+					case GPSDriverUBX::Board::u_blox8:
+						set_device_type(DRV_GPS_DEVTYPE_UBX_8);
+						break;
+
+					case GPSDriverUBX::Board::u_blox9:
+						set_device_type(DRV_GPS_DEVTYPE_UBX_9);
+						break;
+
+					case GPSDriverUBX::Board::u_blox9_F9P:
+						set_device_type(DRV_GPS_DEVTYPE_UBX_F9P);
+						break;
+
+					default:
+						set_device_type(DRV_GPS_DEVTYPE_UBX);
+						break;
+					}
+				}
 			}
 
 			int helper_ret;
@@ -987,6 +1035,8 @@ void
 GPS::publish()
 {
 	if (_instance == Instance::Main || _is_gps_main_advertised.load()) {
+		_report_gps_pos.device_id = get_device_id();
+
 		_report_gps_pos_pub.publish(_report_gps_pos);
 		// Heading/yaw data can be updated at a lower rate than the other navigation data.
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.

src/drivers/uavcan/sensors/gnss.cpp

@@ -64,6 +64,8 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
 	for (uint8_t i = 0; i < _max_channels; i++) {
 		_channel_using_fix2[i] = false;
 	}
+
+	set_device_type(DRV_GPS_DEVTYPE_UAVCAN);
 }
 
 UavcanGnssBridge::~UavcanGnssBridge()
@@ -282,6 +284,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 				    const bool valid_pos_cov, const bool valid_vel_cov)
 {
 	sensor_gps_s report{};
+	report.device_id = get_device_id();
 
 	/*
 	 * FIXME HACK

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -114,6 +114,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report)
 
 		// update device id as we now know our device node_id
 		_device_id.devid_s.address = static_cast<uint8_t>(node_id);
+		_device_id.devid_s.bus_type = DeviceBusType::DeviceBusType_UAVCAN;
 
 		// Publish to the appropriate topic, abort on failure
 		channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->instance);

src/drivers/uavcan/uavcan_servers.cpp

@@ -59,7 +59,7 @@
 
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
-#include <uORB/topics/parameter_request.h>
+#include <uORB/topics/uavcan_parameter_request.h>
 
 /**
  * @file uavcan_servers.cpp
@@ -326,7 +326,7 @@ UavcanServers::run(pthread_addr_t)
 
 		// Check for parameter requests (get/set/list)
 		if (param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
-			parameter_request_s request{};
+			uavcan_parameter_request_s request{};
 			param_request_sub.copy(&request);
 
 			if (_param_counts[request.node_id]) {
@@ -334,7 +334,7 @@ UavcanServers::run(pthread_addr_t)
 				 * We know how many parameters are exposed by this node, so
 				 * process the request.
 				 */
-				if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) {
+				if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) {
 					uavcan::protocol::param::GetSet::Request req;
 
 					if (request.param_index >= 0) {
@@ -354,7 +354,7 @@ UavcanServers::run(pthread_addr_t)
 						_param_index = request.param_index;
 					}
 
-				} else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_SET) {
+				} else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET) {
 					uavcan::protocol::param::GetSet::Request req;
 
 					if (request.param_index >= 0) {
@@ -364,10 +364,10 @@ UavcanServers::run(pthread_addr_t)
 						req.name = (char *)request.param_id;
 					}
 
-					if (request.param_type == parameter_request_s::TYPE_REAL32) {
+					if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_REAL32) {
 						req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
 
-					} else if (request.param_type == parameter_request_s::TYPE_UINT8) {
+					} else if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_UINT8) {
 						req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
 
 					} else {
@@ -387,7 +387,7 @@ UavcanServers::run(pthread_addr_t)
 						_param_index = request.param_index;
 					}
 
-				} else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
+				} else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
 					// This triggers the _param_list_in_progress case below.
 					_param_index = 0;
 					_param_list_in_progress = true;
@@ -397,8 +397,8 @@ UavcanServers::run(pthread_addr_t)
 					PX4_DEBUG("starting component-specific param list");
 				}
 
-			} else if (request.node_id == parameter_request_s::NODE_ID_ALL) {
-				if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
+			} else if (request.node_id == uavcan_parameter_request_s::NODE_ID_ALL) {
+				if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
 					/*
 					 * This triggers the _param_list_in_progress case below,
 					 * but additionally iterates over all active nodes.
@@ -631,13 +631,13 @@ UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param
 	} else {
 		/*
 		 * Currently in parameter get/set mode:
-		 * Publish a uORB parameter_value message containing the current value
+		 * Publish a uORB uavcan_parameter_value message containing the current value
 		 * of the parameter.
 		 */
 		if (result.isSuccessful()) {
 			uavcan::protocol::param::GetSet::Response param = result.getResponse();
 
-			parameter_value_s response{};
+			struct uavcan_parameter_value_s response;
 			response.node_id = result.getCallID().server_node_id.get();
 			strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1);
 			response.param_id[16] = '\0';
@@ -645,15 +645,15 @@ UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param
 			response.param_count = _param_counts[response.node_id];
 
 			if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
-				response.param_type = parameter_request_s::TYPE_INT64;
+				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_INT64;
 				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
 
 			} else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
-				response.param_type = parameter_request_s::TYPE_REAL32;
+				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_REAL32;
 				response.real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
 
 			} else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
-				response.param_type = parameter_request_s::TYPE_UINT8;
+				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_UINT8;
 				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
 			}
 

src/drivers/uavcan/uavcan_servers.hpp

@@ -38,7 +38,7 @@
 #include <drivers/device/Device.hpp>
 #include <lib/perf/perf_counter.h>
 #include <uORB/Publication.hpp>
-#include <uORB/topics/parameter_value.h>
+#include <uORB/topics/uavcan_parameter_value.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
 #include <uavcan/node/sub_node.hpp>
@@ -162,7 +162,7 @@ private:
 	bool _cmd_in_progress = false;
 
 	// uORB topic handle for MAVLink parameter responses
-	uORB::Publication<parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
+	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
 	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	typedef uavcan::MethodBinder<UavcanServers *,

src/lib/parameters/CMakeLists.txt

@@ -151,12 +151,10 @@ if (NOT "${PX4_BOARD}" MATCHES "px4_io")
 	add_library(parameters
 		${SRCS}
 		px4_parameters.hpp
-		ParametersServer.cpp
-		ParametersServer.hpp
 	)
 
 	target_link_libraries(parameters PRIVATE perf tinybson px4_layer)
-	target_compile_definitions(parameters PRIVATE -DMODULE_NAME="parameters")
+	target_compile_definitions(parameters PRIVATE -DMODULE_NAME="parameters" -DDEBUG_BUILD)
 	target_compile_options(parameters
 		PRIVATE
 			-Wno-cast-align		# TODO: fix and enable

src/lib/parameters/ParametersServer.cpp

@@ -1,187 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "ParametersServer.hpp"
-
-#include <px4_platform_common/atomic.h>
-
-#include "param.h"
-
-static ParametersServer *_param_autosave{nullptr};
-
-float ParametersServer::last_autosave_elapsed() const
-{
-	return hrt_elapsed_time_atomic(&_last_autosave_timestamp) * 1e-6f;
-}
-
-void ParametersServer::AutoSave()
-{
-	if (_param_autosave == nullptr) {
-		return;
-	}
-
-	// wait at least 300ms before saving, because:
-	// - tasks often call param_set() for multiple params, so this avoids unnecessary save calls
-	// - the logger stores changed params. He gets notified on a param change via uORB and then
-	//   looks at all unsaved params.
-	float delay = 0.3f;
-
-	static constexpr float rate_limit = 2.0f; // rate-limit saving to 2 seconds
-	const float last_save_elapsed = _param_autosave->last_autosave_elapsed();
-
-	if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) {
-		delay = rate_limit - last_save_elapsed;
-	}
-
-	uint64_t delay_us = delay * 1e6f;
-	_param_autosave->ScheduleDelayed(delay_us);
-}
-
-bool ParametersServer::EnableAutoSave(bool enable)
-{
-	if (enable) {
-		if (_param_autosave == nullptr) {
-			_param_autosave = new ParametersServer();
-
-			if (_param_autosave == nullptr) {
-				PX4_ERR("ParametersServer alloc failed");
-				return false;
-			}
-		}
-
-	} else {
-		// TODO: how to prevent delete if currently running?
-		delete _param_autosave;
-		_param_autosave = nullptr;
-	}
-
-	return true;
-}
-
-void ParametersServer::print_status()
-{
-	if (_param_autosave) {
-		PX4_INFO("last auto save: %.3f seconds ago", (double)_param_autosave->last_autosave_elapsed());
-
-	} else {
-		PX4_INFO("auto save: off");
-	}
-}
-
-void ParametersServer::Run()
-{
-	_last_autosave_timestamp = hrt_absolute_time();
-	int ret = param_save_default();
-
-	if (ret != 0) {
-		PX4_ERR("param auto save failed (%i)", ret);
-	}
-
-	// Check for parameter requests (get/set/list)
-	if (_param_request_sub.updated()) {
-		parameter_request_s request{};
-		param_request_sub.copy(&request);
-
-		/*
-		 * We know how many parameters are exposed by this node, so
-		 * process the request.
-		 */
-		if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) {
-			parameter_value_s parameter_value{};
-
-			if (request.param_index >= 0) {
-				param_for_index(request.param_index);
-
-			} else {
-				param_find(&request.param_id);
-			}
-
-			int call_res = _param_getset_client.call(request.node_id, req);
-
-			if (call_res < 0) {
-				PX4_ERR("couldn't send GetSet: %d", call_res);
-
-			} else {
-				_param_in_progress = true;
-				_param_index = request.param_index;
-			}
-
-		} else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_SET) {
-			uavcan::protocol::param::GetSet::Request req;
-
-			if (request.param_index >= 0) {
-				req.index = request.param_index;
-
-			} else {
-				req.name = (char *)request.param_id;
-			}
-
-			if (request.param_type == parameter_request_s::TYPE_REAL32) {
-				req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
-
-			} else if (request.param_type == parameter_request_s::TYPE_UINT8) {
-				req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
-
-			} else {
-				req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
-			}
-
-		} else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
-			// This triggers the _param_list_in_progress case below.
-			_param_index = 0;
-			_param_list_in_progress = true;
-			_param_list_node_id = request.node_id;
-			_param_list_all_nodes = false;
-
-			PX4_DEBUG("starting component-specific param list");
-
-		} else if (request.node_id == parameter_request_s::NODE_ID_ALL) {
-			if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
-				/*
-				 * This triggers the _param_list_in_progress case below,
-				 * but additionally iterates over all active nodes.
-				 */
-				_param_index = 0;
-				_param_list_in_progress = true;
-				_param_list_node_id = get_next_active_node_id(0);
-				_param_list_all_nodes = true;
-
-				PX4_DEBUG("starting global param list with node %hhu", _param_list_node_id);
-
-				if (_param_counts[_param_list_node_id] == 0) {
-					param_count(_param_list_node_id);
-				}
-			}
-		}
-	}
-}

src/lib/parameters/ParametersServer.hpp

@@ -1,71 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-
-#include <uORB/topics/parameter_request.h>
-#include <uORB/topics/parameter_value.h>
-#include <uORB/Publication.hpp>
-#include <uORB/SubscriptionCallback.hpp>
-
-/**
- * Automatically save the parameters after a timeout and limited rate.
- *
- * This needs to be called with the writer lock held (it's not necessary that it's the writer lock, but it
- * needs to be the same lock as autosave_worker() and param_control_autosave() use).
- */
-
-class ParametersServer : public px4::ScheduledWorkItem
-{
-public:
-	ParametersServer() : px4::ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) {}
-	~ParametersServer() override = default;
-
-	float last_autosave_elapsed() const;
-
-	static void AutoSave();
-	static bool EnableAutoSave(bool enable = true);
-
-	static void print_status();
-
-private:
-	void Run() override;
-
-	hrt_abstime _last_autosave_timestamp{0};
-
-	uORB::Publication<parameter_value_s> _param_response_pub{ORB_ID(parameter_value)};
-	uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)};
-
-};

src/lib/parameters/flashparams/flashparams.cpp

@@ -117,7 +117,7 @@ param_export_internal(bool only_unsaved, param_filter_func filter)
 		case PARAM_TYPE_INT32:
 			i = s->val.i;
 
-			if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
+			if (bson_encoder_append_int32(&encoder, param_name(s->param), i)) {
 				debug("BSON append failed for '%s'", param_name(s->param));
 				goto out;
 			}

src/lib/parameters/parameters.cpp

@@ -89,7 +89,12 @@ static char *param_user_file = nullptr;
 #define PARAM_CLOSE	close
 #endif
 
-#include "ParametersServer.hpp"
+#include <px4_platform_common/workqueue.h>
+/* autosaving variables */
+static hrt_abstime last_autosave_timestamp = 0;
+static struct work_s autosave_work {};
+static px4::atomic<bool> autosave_scheduled{false};
+static bool autosave_disabled = false;
 
 static constexpr uint16_t param_info_count = sizeof(px4::parameters) / sizeof(param_info_s);
 static px4::AtomicBitset<param_info_count> params_active;  // params found
@@ -194,8 +199,6 @@ param_init()
 	param_find_perf = perf_alloc(PC_COUNT, "param: find");
 	param_get_perf = perf_alloc(PC_COUNT, "param: get");
 	param_set_perf = perf_alloc(PC_ELAPSED, "param: set");
-
-	param_control_autosave(true);
 }
 
 /**
@@ -642,6 +645,44 @@ param_get_system_default_value(param_t param, void *default_val)
 	return ret;
 }
 
+/**
+ * worker callback method to save the parameters
+ * @param arg unused
+ */
+static void
+autosave_worker(void *arg)
+{
+	bool disabled = false;
+
+	if (!param_get_default_file()) {
+		// In case we save to FLASH, defer param writes until disarmed,
+		// as writing to FLASH can stall the entire CPU (in rare cases around 300ms on STM32F7)
+		uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
+
+		if (armed_sub.get().armed) {
+			work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(1_s));
+			return;
+		}
+	}
+
+	param_lock_writer();
+	last_autosave_timestamp = hrt_absolute_time();
+	autosave_scheduled.store(false);
+	disabled = autosave_disabled;
+	param_unlock_writer();
+
+	if (disabled) {
+		return;
+	}
+
+	PX4_DEBUG("Autosaving params");
+	int ret = param_save_default();
+
+	if (ret != 0) {
+		PX4_ERR("param auto save failed (%i)", ret);
+	}
+}
+
 /**
  * Automatically save the parameters after a timeout and limited rate.
  *
@@ -651,14 +692,38 @@ param_get_system_default_value(param_t param, void *default_val)
 static void
 param_autosave()
 {
-	ParametersServer::AutoSave();
+	if (autosave_scheduled.load() || autosave_disabled) {
+		return;
+	}
+
+	// wait at least 300ms before saving, because:
+	// - tasks often call param_set() for multiple params, so this avoids unnecessary save calls
+	// - the logger stores changed params. He gets notified on a param change via uORB and then
+	//   looks at all unsaved params.
+	hrt_abstime delay = 300_ms;
+
+	static constexpr const hrt_abstime rate_limit = 2_s; // rate-limit saving to 2 seconds
+	const hrt_abstime last_save_elapsed = hrt_elapsed_time(&last_autosave_timestamp);
+
+	if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) {
+		delay = rate_limit - last_save_elapsed;
+	}
+
+	autosave_scheduled.store(true);
+	work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay));
 }
 
 void
 param_control_autosave(bool enable)
 {
 	param_lock_writer();
-	ParametersServer::EnableAutoSave(enable);
+
+	if (!enable && autosave_scheduled.load()) {
+		work_cancel(LPWORK, &autosave_work);
+		autosave_scheduled.store(false);
+	}
+
+	autosave_disabled = !enable;
 	param_unlock_writer();
 }
 
@@ -944,6 +1009,8 @@ int param_reset_no_notification(param_t param) { return param_reset_internal(par
 static void
 param_reset_all_internal(bool auto_save)
 {
+	PX4_INFO("param_reset_all_internal");
+
 	param_lock_writer();
 
 	if (param_values != nullptr) {
@@ -1066,33 +1133,42 @@ int param_save_default()
 		return res;
 	}
 
-	/* write parameters to temp file */
-	int fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);
-
-	if (fd < 0) {
-		PX4_ERR("failed to open param file: %s", filename);
-
-		return PX4_ERROR;
-	}
-
 	int attempts = 5;
 
 	while (res != OK && attempts > 0) {
-		res = param_export(fd, false, nullptr);
-		attempts--;
 
-		if (res != PX4_OK) {
-			PX4_ERR("param_export failed, retrying %d", attempts);
-			lseek(fd, 0, SEEK_SET); // jump back to the beginning of the file
+		/* write parameters to temp file */
+		int fd = ::open(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);
+
+		if (fd >= 0) {
+			PX4_DEBUG("param export %d/5", attempts - 4);
+			res = param_export(fd, false, nullptr);
+			::close(fd);
+
+			if (res == PX4_OK) {
+				fd = -1;
+
+				static const char *backup_file = PX4_ROOTFSDIR"/eeprom/parameters_backup.bson";
+				fd = ::open(backup_file, O_WRONLY | O_CREAT, PX4_O_MODE_666);
+				res = param_export(fd, false, nullptr);
+				::close(fd);
+
+			} else {
+				PX4_ERR("param_export failed, retrying %d", attempts);
+			}
+
+		} else {
+			PX4_ERR("failed to open param file: %s", filename);
+			return PX4_ERROR;
 		}
+
+		attempts--;
 	}
 
 	if (res != OK) {
 		PX4_ERR("failed to write parameters to file: %s", filename);
 	}
 
-	PARAM_CLOSE(fd);
-
 	return res;
 }
 
@@ -1135,20 +1211,19 @@ param_load_default()
 int
 param_export(int fd, bool only_unsaved, param_filter_func filter)
 {
-	int	result = -1;
 	perf_begin(param_export_perf);
 
 	if (fd < 0) {
 		param_lock_writer();
 		// flash_param_save() will take the shutdown lock
-		result = flash_param_save(only_unsaved, filter);
+		int result = flash_param_save(only_unsaved, filter);
 		param_unlock_writer();
 		perf_end(param_export_perf);
 		return result;
 	}
 
-	param_wbuf_s *s = nullptr;
-	struct bson_encoder_s encoder;
+
+
 
 	int shutdown_lock_ret = px4_shutdown_lock();
 
@@ -1161,16 +1236,31 @@ param_export(int fd, bool only_unsaved, param_filter_func filter)
 
 	param_lock_reader();
 
+	bson_encoder_s encoder{};
 	uint8_t bson_buffer[256];
 	bson_encoder_init_buf_file(&encoder, fd, &bson_buffer, sizeof(bson_buffer));
 
+	int result = PX4_ERROR;
+	param_wbuf_s *s = nullptr;
+	uint32_t export_crc32 = 0;
+	uint32_t export_count_int32 = 0;
+	uint32_t export_count_double = 0;
+	size_t total_size = sizeof(int32_t);
+
 	/* no modified parameters -> we are done */
 	if (param_values == nullptr) {
-		result = 0;
+		result = PX4_OK;
 		goto out;
 	}
 
-	while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != nullptr) {
+	while ((s = (param_wbuf_s *)utarray_next(param_values, s)) != nullptr) {
+
+		if (encoder.dead) {
+			PX4_ERR("bson encoder dead!");
+			result = PX4_ERROR;
+			goto out;
+		}
+
 		/*
 		 * If we are only saving values changed since last save, and this
 		 * one hasn't, then skip it
@@ -1210,28 +1300,39 @@ param_export(int fd, bool only_unsaved, param_filter_func filter)
 
 		s->unsaved = false;
 
-		const char *name = param_name(s->param);
-		const size_t size = param_size(s->param);
-
 		/* append the appropriate BSON type object */
 		switch (param_type(s->param)) {
-
 		case PARAM_TYPE_INT32: {
+				const char *name = param_name(s->param);
 				const int32_t i = s->val.i;
-				PX4_DEBUG("exporting: %s (%d) size: %lu val: %d", name, s->param, (long unsigned int)size, i);
+				size_t bson_size = sizeof(int8_t) + strlen(name) + 1 + sizeof(i);
+				total_size += bson_size;
+				PX4_DEBUG("exporting: %s (%d) (name size: %d) size: %lu val: %d - size: %d, total_size: %d", name, s->param,
+					  strlen(name) + 1, (long unsigned int)param_size(s->param), i, bson_size, total_size);
 
-				if (bson_encoder_append_int(&encoder, name, i)) {
+				if (bson_encoder_append_int32(&encoder, name, i) == 0) {
+					export_count_int32++;
+
+				} else {
 					PX4_ERR("BSON append failed for '%s'", name);
 					goto out;
 				}
+
 			}
 			break;
 
 		case PARAM_TYPE_FLOAT: {
+				const char *name = param_name(s->param);
 				const double f = (double)s->val.f;
-				PX4_DEBUG("exporting: %s (%d) size: %lu val: %.3f", name, s->param, (long unsigned int)size, (double)f);
+				size_t bson_size = sizeof(int8_t) + strlen(name) + 1 + sizeof(f);
+				total_size += bson_size;
+				PX4_DEBUG("exporting: %s (%d) (name size: %d) size: %lu val: %.3f - size: %d, total_size: %d", name, s->param,
+					  strlen(name) + 1, (long unsigned int)param_size(s->param), (double)f, bson_size, total_size);
 
-				if (bson_encoder_append_double(&encoder, name, f)) {
+				if (bson_encoder_append_double(&encoder, name, f) == 0) {
+					export_count_double++;
+
+				} else {
 					PX4_ERR("BSON append failed for '%s'", name);
 					goto out;
 				}
@@ -1242,16 +1343,29 @@ param_export(int fd, bool only_unsaved, param_filter_func filter)
 			PX4_ERR("unrecognized parameter type");
 			goto out;
 		}
+
+		const char *name = param_name(s->param);
+		const void *val = &(s->val);
+		export_crc32 = crc32part((const uint8_t *)name, strlen(name), export_crc32);
+		export_crc32 = crc32part((const uint8_t *)val, param_size(s->param), export_crc32);
 	}
 
-	result = 0;
+	result = PX4_OK;
 
 out:
 
-	if (result == 0) {
+	if (result == PX4_OK) {
+		bson_encoder_append_int32(&encoder, "PX4_PARAMETER_CRC32", export_crc32);
+		bson_encoder_append_int32(&encoder, "PX4_PARAMETER_COUNT_INT32", export_count_int32);
+		bson_encoder_append_int32(&encoder, "PX4_PARAMETER_COUNT_DOUBLE", export_count_double);
+
+		PX4_DEBUG("bson_encoder_fini");
+
 		if (bson_encoder_fini(&encoder) != PX4_OK) {
 			PX4_ERR("bson encoder finish failed");
 		}
+
+		PX4_DEBUG("BSON final size = %d, crc32: %X", encoder.total_written_size, export_crc32);
 	}
 
 	param_unlock_reader();
@@ -1308,16 +1422,22 @@ param_import_callback(bson_decoder_t decoder, void *priv, bson_node_t node)
 
 	switch (node->type) {
 	case BSON_INT32: {
-			if (param_type(param) != PARAM_TYPE_INT32) {
-				PX4_WARN("unexpected type for %s", node->name);
-				result = 1; // just skip this entry
-				goto out;
+			if (strcmp(node->name, "PX4_PARAMETER_CRC32") == 0) {
+				int32_t crc32 = node->i;
+				PX4_INFO("%s = %X", node->name, crc32);
+
+			} else {
+				if (param_type(param) != PARAM_TYPE_INT32) {
+					PX4_WARN("unexpected type for %s", node->name);
+					result = 1; // just skip this entry
+					goto out;
+				}
+
+				i = node->i;
+				v = &i;
+
+				PX4_DEBUG("Imported %s with value %d", param_name(param), i);
 			}
-
-			i = node->i;
-			v = &i;
-
-			PX4_DEBUG("Imported %s with value %d", param_name(param), i);
 		}
 		break;
 
@@ -1340,6 +1460,11 @@ param_import_callback(bson_decoder_t decoder, void *priv, bson_node_t node)
 		goto out;
 	}
 
+	{
+		decoder->import_scratch = crc32part((const uint8_t *)node->name, strlen(node->name), decoder->import_scratch);
+		decoder->import_scratch = crc32part((const uint8_t *)v, param_size(param), decoder->import_scratch);
+	}
+
 	if (param_set_internal(param, v, state->mark_saved, true)) {
 		PX4_DEBUG("error setting value for '%s'", node->name);
 		goto out;
@@ -1355,6 +1480,11 @@ out:
 static int
 param_import_internal(int fd, bool mark_saved)
 {
+
+	off_t fsize = lseek(fd, 0, SEEK_END);
+	PX4_DEBUG("param file size = %d", fsize);
+	lseek(fd, 0, SEEK_SET);
+
 	bson_decoder_s decoder;
 	param_import_state state;
 	int result = -1;
@@ -1369,7 +1499,9 @@ param_import_internal(int fd, bool mark_saved)
 	do {
 		result = bson_decoder_next(&decoder);
 
-	} while (result > 0);
+	} while (result > 0 && !decoder.dead);
+
+	PX4_DEBUG("bson decode finished total bytes: %d, crc32: %X", decoder.total_bytes_imported, decoder.import_scratch);
 
 	return result;
 }
@@ -1462,7 +1594,11 @@ void param_print_status()
 			 param_custom_default_values->n * sizeof(UT_icd));
 	}
 
-	ParametersServer::print_status();
+	PX4_INFO("auto save: %s", autosave_disabled ? "off" : "on");
+
+	if (!autosave_disabled && (last_autosave_timestamp > 0)) {
+		PX4_INFO("last auto save: %.3f seconds ago", hrt_elapsed_time(&last_autosave_timestamp) * 1e-6);
+	}
 
 	perf_print_counter(param_export_perf);
 	perf_print_counter(param_find_perf);

src/lib/parameters/parameters_shmem.cpp

@@ -1149,7 +1149,7 @@ param_export(int fd, bool only_unsaved, param_filter_func filter)
 
 				PX4_DEBUG("exporting: %s (%d) size: %d val: %d", name, s->param, size, i);
 
-				if (bson_encoder_append_int(&encoder, name, i)) {
+				if (bson_encoder_append_int32(&encoder, name, i)) {
 					PX4_ERR("BSON append failed for '%s'", name);
 					goto out;
 				}

src/lib/parameters/tinybson/CMakeLists.txt

@@ -33,5 +33,5 @@
 
 add_library(tinybson tinybson.cpp)
 target_compile_definitions(tinybson PRIVATE -DMODULE_NAME="tinybson")
-target_compile_options(tinybson PRIVATE -Wno-sign-compare) # TODO: fix this
+target_compile_options(tinybson PRIVATE -Wno-sign-compare -Wno-error) # TODO: fix this
 add_dependencies(tinybson prebuild_targets)

src/lib/parameters/tinybson/tinybson.cpp

@@ -45,7 +45,7 @@
 
 #include "tinybson.h"
 
-#if 0
+#if 1
 # define debug(fmt, args...)		do { PX4_INFO("BSON: " fmt, ##args); } while(0)
 #else
 # define debug(fmt, args...)		do { } while(0)
@@ -64,7 +64,14 @@ read_x(bson_decoder_t decoder, void *p, size_t s)
 	CODER_CHECK(decoder);
 
 	if (decoder->fd > -1) {
-		return (BSON_READ(decoder->fd, p, s) == s) ? 0 : -1;
+		int ret = BSON_READ(decoder->fd, p, s);
+
+		if (ret == s) {
+			decoder->total_bytes_imported += ret;
+			return 0;
+		}
+
+		return -1;
 	}
 
 	if (decoder->buf != nullptr) {
@@ -79,6 +86,7 @@ read_x(bson_decoder_t decoder, void *p, size_t s)
 
 		memcpy(p, (decoder->buf + decoder->bufpos), s);
 		decoder->bufpos += s;
+		decoder->total_bytes_imported += s;
 		return 0;
 	}
 
@@ -113,8 +121,6 @@ read_double(bson_decoder_t decoder, double *d)
 int
 bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *priv)
 {
-	int32_t	junk;
-
 	decoder->fd = fd;
 	decoder->buf = nullptr;
 	decoder->dead = false;
@@ -125,10 +131,14 @@ bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback cal
 	decoder->node.type = BSON_UNDEFINED;
 
 	/* read and discard document size */
-	if (read_int32(decoder, &junk)) {
+	int32_t	junk_size = 0;
+
+	if (read_int32(decoder, &junk_size)) {
 		CODER_KILL(decoder, "failed discarding length");
 	}
 
+	debug("junk_size = %d", junk_size);
+
 	/* ready for decoding */
 	return 0;
 }
@@ -340,7 +350,14 @@ write_x(bson_encoder_t encoder, const void *p, size_t s)
 
 	/* bson file encoder (non-buffered) */
 	if (encoder->fd > -1 && encoder->buf == nullptr) {
-		return (BSON_WRITE(encoder->fd, p, s) == (int)s) ? 0 : -1;
+		int ret = BSON_WRITE(encoder->fd, p, s);
+
+		if (ret == s) {
+			encoder->total_written_size += ret;
+			return 0;
+		}
+
+		return -1;
 	}
 
 	/* do we need to extend the buffer? */
@@ -353,6 +370,8 @@ write_x(bson_encoder_t encoder, const void *p, size_t s)
 			int ret = BSON_WRITE(encoder->fd, encoder->buf, encoder->bufpos);
 
 			if (ret == (int)encoder->bufpos) {
+				encoder->total_written_size += ret;
+
 				// reset buffer to beginning and continue
 				encoder->bufpos = 0;
 
@@ -432,6 +451,8 @@ bson_encoder_init_file(bson_encoder_t encoder, int fd)
 	encoder->buf = nullptr;
 	encoder->dead = false;
 
+	encoder->total_written_size = 0;
+
 	if (write_int32(encoder, 0)) {
 		CODER_KILL(encoder, "write error on document length");
 	}
@@ -449,6 +470,8 @@ bson_encoder_init_buf_file(bson_encoder_t encoder, int fd, void *buf, unsigned b
 	encoder->dead = false;
 	encoder->realloc_ok = false;
 
+	encoder->total_written_size = 0;
+
 	if (write_int32(encoder, 0)) {
 		CODER_KILL(encoder, "write error on document length");
 	}
@@ -473,6 +496,8 @@ bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize)
 		encoder->realloc_ok = false;
 	}
 
+	encoder->total_written_size = 0;
+
 	if (write_int32(encoder, 0)) {
 		CODER_KILL(encoder, "write error on document length");
 	}
@@ -489,13 +514,30 @@ bson_encoder_fini(bson_encoder_t encoder)
 		CODER_KILL(encoder, "write error on document terminator");
 	}
 
-	if (encoder->fd > -1 && encoder->buf != nullptr) {
-		/* write final buffer to disk */
+	if (encoder->fd > -1 && encoder->bufpos > 0) {
 		int ret = BSON_WRITE(encoder->fd, encoder->buf, encoder->bufpos);
 
-		if (ret != (int)encoder->bufpos) {
+		if (ret == (int)encoder->bufpos) {
+			encoder->total_written_size += ret;
+
+		} else {
 			CODER_KILL(encoder, "write error");
 		}
+	}
+
+	// record document size
+	if (encoder->fd > -1) {
+		off_t offset = lseek(encoder->fd, 0, SEEK_SET);
+
+		int32_t total_document_size = encoder->total_written_size;
+
+		debug("writing document size %d to beginning of file (%d)", total_document_size, offset);
+
+		if (BSON_WRITE(encoder->fd, &total_document_size, sizeof(total_document_size)) != sizeof(total_document_size)) {
+			CODER_KILL(encoder, "write error on document length");
+		}
+
+		BSON_FSYNC(encoder->fd);
 
 	} else if (encoder->buf != nullptr) {
 		/* update buffer length */
@@ -503,11 +545,6 @@ bson_encoder_fini(bson_encoder_t encoder)
 		memcpy(encoder->buf, &len, sizeof(len));
 	}
 
-	/* sync file */
-	if (encoder->fd > -1) {
-		BSON_FSYNC(encoder->fd);
-	}
-
 	return 0;
 }
 
@@ -549,25 +586,17 @@ int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool valu
 }
 
 int
-bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value)
+bson_encoder_append_int32(bson_encoder_t encoder, const char *name, int32_t value)
 {
 	bool result;
 
 	CODER_CHECK(encoder);
 
 	/* use the smallest encoding that will hold the value */
-	if (value == (int32_t)value) {
-		debug("encoding %lld as int32", value);
-		result = write_int8(encoder, BSON_INT32) ||
-			 write_name(encoder, name) ||
-			 write_int32(encoder, value);
-
-	} else {
-		debug("encoding %lld as int64", value);
-		result = write_int8(encoder, BSON_INT64) ||
-			 write_name(encoder, name) ||
-			 write_int64(encoder, value);
-	}
+	debug("encoding %lld as int32", value);
+	result = write_int8(encoder, BSON_INT32) ||
+		 write_name(encoder, name) ||
+		 write_int32(encoder, value);
 
 	if (result) {
 		CODER_KILL(encoder, "write error on BSON_INT");

src/lib/parameters/tinybson/tinybson.h

@@ -103,19 +103,22 @@ typedef int	(* bson_decoder_callback)(bson_decoder_t decoder, void *priv, bson_n
 
 struct bson_decoder_s {
 	/* file reader state */
-	int			fd;
+	int			fd{-1};
 
 	/* buffer reader state */
-	uint8_t			*buf;
-	size_t			bufsize;
-	unsigned		bufpos;
+	uint8_t			*buf{nullptr};
+	size_t			bufsize{0};
+	unsigned		bufpos{0};
+	size_t                  total_bytes_imported{0};
 
-	bool			dead;
-	bson_decoder_callback	callback;
-	void			*priv;
-	unsigned		nesting;
+	bool			dead{false};
+	bson_decoder_callback	callback{nullptr};
+	void			*priv{nullptr};
+	unsigned		nesting{0};
 	struct bson_node_s	node;
-	int32_t			pending;
+	int32_t			pending{0};
+
+	uint32_t                import_scratch{0};
 };
 
 /**
@@ -179,6 +182,8 @@ typedef struct bson_encoder_s {
 	unsigned	bufsize;
 	unsigned	bufpos;
 
+	size_t          total_written_size{0};
+
 	bool		realloc_ok;
 	bool		dead;
 
@@ -252,7 +257,7 @@ __EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name,
  * @param name			Node name.
  * @param value			Value to be encoded.
  */
-__EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value);
+__EXPORT int bson_encoder_append_int32(bson_encoder_t encoder, const char *name, int32_t value);
 
 /**
  * Append a double to the encoded stream

src/lib/systemlib/hardfault_log.h

@@ -41,10 +41,16 @@
 /****************************************************************************
  * Pre-processor Definitions
  ****************************************************************************/
+
+/* The path to the Battery Backed up SRAM */
+#define BBSRAM_PATH "/fs/bbr"
+
 #define HARDFAULT_REBOOT_FILENO 0
 #define HARDFAULT_REBOOT_PATH BBSRAM_PATH "" STRINGIFY(HARDFAULT_REBOOT_FILENO)
+
 #define HARDFAULT_ULOG_FILENO 3
 #define HARDFAULT_ULOG_PATH BBSRAM_PATH "" STRINGIFY(HARDFAULT_ULOG_FILENO)
+
 #define HARDFAULT_FILENO 4
 #define HARDFAULT_PATH BBSRAM_PATH "" STRINGIFY(HARDFAULT_FILENO)
 
@@ -63,20 +69,18 @@
  */
 #define BBSRAM_HEADER_SIZE 20 /* This is an assumption */
 #define BBSRAM_USED ((5*BBSRAM_HEADER_SIZE)+(BBSRAM_SIZE_FN0+BBSRAM_SIZE_FN1+BBSRAM_SIZE_FN2+BBSRAM_SIZE_FN3))
-#define BBSRAM_REAMINING (PX4_BBSRAM_SIZE-BBSRAM_USED)
+#define BBSRAM_REMAINING (PX4_BBSRAM_SIZE-BBSRAM_USED)
 #if CONFIG_ARCH_INTERRUPTSTACK <= 3
 #  define BBSRAM_NUMBER_STACKS 1
 #else
 #  define BBSRAM_NUMBER_STACKS 2
 #endif
 #define BBSRAM_FIXED_ELEMENTS_SIZE (sizeof(info_s))
-#define BBSRAM_LEFTOVER (BBSRAM_REAMINING-BBSRAM_FIXED_ELEMENTS_SIZE)
+#define BBSRAM_LEFTOVER (BBSRAM_REMAINING-BBSRAM_FIXED_ELEMENTS_SIZE)
 
 #define CONFIG_ISTACK_SIZE (BBSRAM_LEFTOVER/BBSRAM_NUMBER_STACKS/sizeof(stack_word_t))
 #define CONFIG_USTACK_SIZE (BBSRAM_LEFTOVER/BBSRAM_NUMBER_STACKS/sizeof(stack_word_t))
 
-/* The path to the Battery Backed up SRAM */
-#define BBSRAM_PATH "/fs/bbr"
 /* The sizes of the files to create (-1) use rest of BBSRAM memory */
 #define BSRAM_FILE_SIZES { \
 		BBSRAM_SIZE_FN0,   /* For Time stamp only */                  \
@@ -256,15 +260,13 @@ typedef struct {
 	fault_regs_s          fault_regs;             /* NVIC status */
 	stack_t               stacks;                 /* Stack info */
 #if CONFIG_TASK_NAME_SIZE > 0
-	char                  name[CONFIG_TASK_NAME_SIZE + 1]; /* Task name (with NULL
-													* terminator) */
+	char                  name[CONFIG_TASK_NAME_SIZE + 1]; /* Task name (with NULL terminator) */
 #endif
-	char                  filename[MAX_FILE_PATH_LENGTH]; /* the Last of chars in
-												  * __FILE__ to up_assert */
+	char                  filename[MAX_FILE_PATH_LENGTH]; /* the Last of chars in __FILE__ to up_assert */
 } info_s;
 
 typedef struct {
-	info_s    info;                       /* The info */
+	info_s    info;                 /* The info */
 #if CONFIG_ARCH_INTERRUPTSTACK > 3      /* The amount of stack data is compile time
 									 * sized backed on what is left after the
 									 * other BBSRAM files are defined

src/modules/mavlink/mavlink_parameters.cpp

@@ -80,7 +80,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
 			    (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
 				// publish list request to UAVCAN driver via uORB.
-				parameter_request_s req{};
+				uavcan_parameter_request_s req{};
 				req.message_type = msg->msgid;
 				req.node_id = req_list.target_component;
 				req.param_index = 0;
@@ -140,7 +140,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
 			    (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
 				// publish set request to UAVCAN driver via uORB.
-				parameter_request_s req{};
+				uavcan_parameter_request_s req{};
 				req.message_type = msg->msgid;
 				req.node_id = set.target_component;
 				req.param_index = -1;
@@ -219,7 +219,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
 			    (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
 				// publish set request to UAVCAN driver via uORB.
-				parameter_request_s req{};
+				uavcan_parameter_request_s req{};
 				req.timestamp = hrt_absolute_time();
 				req.message_type = msg->msgid;
 				req.node_id = req_read.target_component;
@@ -406,7 +406,7 @@ bool
 MavlinkParametersManager::send_uavcan()
 {
 	/* Send parameter values received from the UAVCAN topic */
-	parameter_value_s value{};
+	uavcan_parameter_value_s value{};
 
 	if (_uavcan_parameter_value_sub.update(&value)) {
 
@@ -593,7 +593,7 @@ void MavlinkParametersManager::request_next_uavcan_parameter()
 {
 	// Request a parameter if we are not already waiting on a response and if the list is not empty
 	if (!_uavcan_waiting_for_request_response && _uavcan_open_request_list != nullptr) {
-		parameter_request_s req = _uavcan_open_request_list->req;
+		uavcan_parameter_request_s req = _uavcan_open_request_list->req;
 
 		_uavcan_parameter_request_pub.publish(req);
 
@@ -601,7 +601,7 @@ void MavlinkParametersManager::request_next_uavcan_parameter()
 	}
 }
 
-void MavlinkParametersManager::enque_uavcan_request(parameter_request_s *req)
+void MavlinkParametersManager::enque_uavcan_request(uavcan_parameter_request_s *req)
 {
 	// We store at max 10 requests to keep memory consumption low.
 	// Dropped requests will be repeated by the ground station

src/modules/mavlink/mavlink_parameters.h

@@ -49,8 +49,8 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/rc_parameter_map.h>
-#include <uORB/topics/parameter_request.h>
-#include <uORB/topics/parameter_value.h>
+#include <uORB/topics/uavcan_parameter_request.h>
+#include <uORB/topics/uavcan_parameter_value.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -105,8 +105,8 @@ protected:
 
 	// Item of a single-linked list to store requested uavcan parameters
 	struct _uavcan_open_request_list_item {
-		parameter_request_s req{};
-		_uavcan_open_request_list_item *next{nullptr};
+		uavcan_parameter_request_s req;
+		struct _uavcan_open_request_list_item *next;
 	};
 
 	/**
@@ -117,7 +117,7 @@ protected:
 	/**
 	 * Enqueue one uavcan parameter reqest. We store 10 at max.
 	 */
-	void enque_uavcan_request(parameter_request_s *req);
+	void enque_uavcan_request(uavcan_parameter_request_s *req);
 
 	/**
 	 * Drop the first reqest from the list
@@ -131,22 +131,22 @@ protected:
 	uORB::Publication<rc_parameter_map_s>	_rc_param_map_pub{ORB_ID(rc_parameter_map)};
 	rc_parameter_map_s _rc_param_map{};
 
-	uORB::Publication<parameter_request_s> _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)};
+	uORB::Publication<uavcan_parameter_request_s> _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)};
 	// enforce ORB_ID(uavcan_parameter_request) constants that map to MAVLINK defines
-	static_assert(parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ == MAVLINK_MSG_ID_PARAM_REQUEST_READ,
-		      "parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_READ constant mismatch");
-	static_assert(parameter_request_s::MESSAGE_TYPE_PARAM_SET == MAVLINK_MSG_ID_PARAM_SET,
-		      "parameter_request_s MAVLINK_MSG_ID_PARAM_SET constant mismatch");
-	static_assert(parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST == MAVLINK_MSG_ID_PARAM_REQUEST_LIST,
-		      "parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_LIST constant mismatch");
-	static_assert(parameter_request_s::NODE_ID_ALL == MAV_COMP_ID_ALL,
-		      "parameter_request_s MAV_COMP_ID_ALL constant mismatch");
-	static_assert(parameter_request_s::TYPE_UINT8 == MAV_PARAM_TYPE_UINT8,
-		      "parameter_request_s MAV_PARAM_TYPE_UINT8 constant mismatch");
-	static_assert(parameter_request_s::TYPE_REAL32 == MAV_PARAM_TYPE_REAL32,
-		      "parameter_request_s MAV_PARAM_TYPE_REAL32 constant mismatch");
-	static_assert(parameter_request_s::TYPE_INT64 == MAV_PARAM_TYPE_INT64,
-		      "parameter_request_s MAV_PARAM_TYPE_INT64 constant mismatch");
+	static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ == MAVLINK_MSG_ID_PARAM_REQUEST_READ,
+		      "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_READ constant mismatch");
+	static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET == MAVLINK_MSG_ID_PARAM_SET,
+		      "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_SET constant mismatch");
+	static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST == MAVLINK_MSG_ID_PARAM_REQUEST_LIST,
+		      "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_LIST constant mismatch");
+	static_assert(uavcan_parameter_request_s::NODE_ID_ALL == MAV_COMP_ID_ALL,
+		      "uavcan_parameter_request_s MAV_COMP_ID_ALL constant mismatch");
+	static_assert(uavcan_parameter_request_s::PARAM_TYPE_UINT8 == MAV_PARAM_TYPE_UINT8,
+		      "uavcan_parameter_request_s MAV_PARAM_TYPE_UINT8 constant mismatch");
+	static_assert(uavcan_parameter_request_s::PARAM_TYPE_REAL32 == MAV_PARAM_TYPE_REAL32,
+		      "uavcan_parameter_request_s MAV_PARAM_TYPE_REAL32 constant mismatch");
+	static_assert(uavcan_parameter_request_s::PARAM_TYPE_INT64 == MAV_PARAM_TYPE_INT64,
+		      "uavcan_parameter_request_s MAV_PARAM_TYPE_INT64 constant mismatch");
 
 	uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -2232,6 +2232,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 
 	sensor_gps_s gps{};
 
+	device::Device::DeviceId device_id{};
+	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
+	device_id.devid_s.address = msg->sysid;
+	device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
+	gps.device_id = device_id.devid;
+
 	gps.lat = hil_gps.lat;
 	gps.lon = hil_gps.lon;
 	gps.alt = hil_gps.alt;

src/modules/sih/sih.cpp

@@ -46,6 +46,7 @@
 #include <px4_platform_common/log.h>
 
 #include <drivers/drv_pwm_output.h>         // to get PWM flags
+#include <lib/drivers/device/Device.hpp>
 
 using namespace math;
 using namespace matrix;
@@ -364,6 +365,14 @@ void Sih::send_gps()
 		gps_no_fix();
 	}
 
+	// device id
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
+	device_id.devid_s.bus = 0;
+	device_id.devid_s.address = 0;
+	device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
+	_sensor_gps.device_id = device_id.devid;
+
 	_sensor_gps_pub.publish(_sensor_gps);
 }
 

src/modules/simulator/simulator_mavlink.cpp

@@ -44,6 +44,7 @@
 #include <drivers/drv_pwm_output.h>
 #include <conversion/rotation.h>
 #include <mathlib/mathlib.h>
+#include <lib/drivers/device/Device.hpp>
 
 #include <arpa/inet.h>
 #include <errno.h>
@@ -432,6 +433,14 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
 			if (_sensor_gps_pubs[i] == nullptr) {
 				_sensor_gps_pubs[i] = new uORB::PublicationMulti<sensor_gps_s> {ORB_ID(sensor_gps)};
 				_gps_ids[i] = hil_gps.id;
+
+				device::Device::DeviceId device_id;
+				device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
+				device_id.devid_s.bus = 0;
+				device_id.devid_s.address = i;
+				device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
+				gps.device_id = device_id.devid;
+
 				_sensor_gps_pubs[i]->publish(gps);
 				break;
 			}

src/systemcmds/hardfault_log/hardfault_log.c

@@ -61,39 +61,8 @@
 
 #include "chip.h"
 
-
-/****************************************************************************
- * Public Function Prototypes
- ****************************************************************************/
-__EXPORT int hardfault_log_main(int argc, char *argv[]);
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
 #define OUT_BUFFER_LEN  200
 
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-static int genfault(int fault);
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-/****************************************************************************
- * genfault
- ****************************************************************************/
-
 static int genfault(int fault)
 {
 
@@ -140,7 +109,7 @@ bool verify_ram_address(uint32_t bot, uint32_t size)
 {
 	bool ret = true;
 #if defined STM32_IS_SRAM
-	ret =  STM32_IS_SRAM(bot) && STM32_IS_SRAM(bot +  size);
+	ret = STM32_IS_SRAM(bot) && STM32_IS_SRAM(bot + size);
 #endif
 	return ret;
 }
@@ -213,7 +182,6 @@ static void identify(const char *caller)
 	}
 }
 
-
 /****************************************************************************
  * hardfault_get_desc
  ****************************************************************************/
@@ -469,8 +437,7 @@ static int write_registers_info(int fdout, info_s *pi, char *buffer, int sz)
 /****************************************************************************
  * write_interrupt_stack_info
  ****************************************************************************/
-static int write_interrupt_stack_info(int fdout, info_s *pi, char *buffer,
-				      unsigned int sz)
+static int write_interrupt_stack_info(int fdout, info_s *pi, char *buffer, unsigned int sz)
 {
 	int ret = ENOENT;
 
@@ -502,8 +469,7 @@ static int write_user_stack_info(int fdout, info_s *pi, char *buffer,
 /****************************************************************************
  * write_dump_info
  ****************************************************************************/
-static int write_dump_info(int fdout, info_s *info, struct bbsramd_s *desc,
-			   char *buffer, unsigned int sz)
+static int write_dump_info(int fdout, info_s *info, struct bbsramd_s *desc, char *buffer, unsigned int sz)
 {
 	char fmtbuff[ TIME_FMT_LEN + 1];
 	format_fault_time(HEADER_TIME_FMT, &desc->lastwrite, fmtbuff, sizeof(fmtbuff));
@@ -528,11 +494,9 @@ static int write_dump_info(int fdout, info_s *info, struct bbsramd_s *desc,
 	}
 
 #ifdef CONFIG_TASK_NAME_SIZE
-	n = snprintf(buffer, sz, " in file:%s at line: %d running task: %s\n",
-		     info->filename, info->lineno, info->name);
+	n = snprintf(buffer, sz, " in file:%s at line: %d running task: %s\n", info->filename, info->lineno, info->name);
 #else
-	n = snprintf(buffer, sz, " in file:%s at line: %d \n",
-		     info->filename, info->lineno);
+	n = snprintf(buffer, sz, " in file:%s at line: %d \n", info->filename, info->lineno);
 #endif
 
 	if (n != write(fdout, buffer, n)) {
@@ -563,8 +527,7 @@ static int write_dump_info(int fdout, info_s *info, struct bbsramd_s *desc,
 /****************************************************************************
  * write_dump_time
  ****************************************************************************/
-static int write_dump_time(char *caller, char *tag, int fdout,
-			   struct timespec *ts, char *buffer, unsigned int sz)
+static int write_dump_time(char *caller, char *tag, int fdout, struct timespec *ts, char *buffer, unsigned int sz)
 {
 	int ret = OK;
 	char fmtbuff[ TIME_FMT_LEN + 1];
@@ -577,27 +540,27 @@ static int write_dump_time(char *caller, char *tag, int fdout,
 
 	return ret;
 }
+
 /****************************************************************************
  * write_dump_footer
  ****************************************************************************/
-static int write_dump_header(char *caller, int fdout, struct timespec *ts,
-			     char *buffer, unsigned int sz)
+static int write_dump_header(char *caller, int fdout, struct timespec *ts, char *buffer, unsigned int sz)
 {
 	return write_dump_time(caller, "Begin", fdout, ts, buffer, sz);
 }
+
 /****************************************************************************
  * write_dump_footer
  ****************************************************************************/
-static int write_dump_footer(char *caller, int fdout, struct timespec *ts,
-			     char *buffer, unsigned int sz)
+static int write_dump_footer(char *caller, int fdout, struct timespec *ts, char *buffer, unsigned int sz)
 {
 	return write_dump_time(caller, "END", fdout, ts, buffer, sz);
 }
+
 /****************************************************************************
  * write_intterupt_satck
  ****************************************************************************/
-static int write_intterupt_stack(int fdin, int fdout, info_s *pi, char *buffer,
-				 unsigned int sz)
+static int write_intterupt_stack(int fdin, int fdout, info_s *pi, char *buffer, unsigned int sz)
 {
 	int ret = ENOENT;
 
@@ -615,12 +578,10 @@ static int write_intterupt_stack(int fdin, int fdout, info_s *pi, char *buffer,
 	return ret;
 }
 
-
 /****************************************************************************
  * write_user_stack
  ****************************************************************************/
-static int write_user_stack(int fdin, int fdout, info_s *pi, char *buffer,
-			    unsigned int sz)
+static int write_user_stack(int fdin, int fdout, info_s *pi, char *buffer, unsigned int sz)
 {
 	int ret = ENOENT;
 
@@ -646,7 +607,6 @@ static int write_user_stack(int fdin, int fdout, info_s *pi, char *buffer,
  */
 static int hardfault_append_to_ulog(const char *caller, int fdin)
 {
-
 	int ret = 0;
 	int write_size_remaining = lseek(fdin, 0, SEEK_END);
 
@@ -769,7 +729,6 @@ static int hardfault_append_to_ulog(const char *caller, int fdin)
 		goto out;
 	}
 
-
 	// now append the data
 	lseek(ulog_fd, 0, SEEK_END);
 
@@ -835,7 +794,6 @@ out:
 	return ret;
 }
 
-
 /****************************************************************************
  * commit
  ****************************************************************************/
@@ -848,7 +806,6 @@ static int hardfault_commit(char *caller)
 	ret = hardfault_get_desc(caller, &desc, false);
 
 	if (ret >= 0) {
-
 		int fd = ret;
 		state = (desc.lastwrite.tv_sec || desc.lastwrite.tv_nsec) ?  OK : 1;
 		int rv = close(fd);
@@ -858,7 +815,6 @@ static int hardfault_commit(char *caller)
 			syslog(LOG_INFO, "Failed to Close Fault Log (%d)\n", rv);
 
 		} else {
-
 			if (state != OK) {
 				identify(caller);
 				syslog(LOG_INFO, "Nothing to save\n", path);
@@ -910,12 +866,10 @@ static int hardfault_commit(char *caller)
 	return ret;
 }
 
-
 /****************************************************************************
  * hardfault_dowrite
  ****************************************************************************/
-static int hardfault_dowrite(char *caller, int infd, int outfd,
-			     struct bbsramd_s *desc, int format)
+static int hardfault_dowrite(char *caller, int infd, int outfd, struct bbsramd_s *desc, int format)
 {
 	int ret = -ENOMEM;
 	char *line = zalloc(OUT_BUFFER_LEN);
@@ -1009,10 +963,6 @@ static int hardfault_dowrite(char *caller, int infd, int outfd,
 	return ret;
 }
 
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
 /****************************************************************************
  * hardfault_rearm
  ****************************************************************************/
@@ -1101,7 +1051,6 @@ __EXPORT int hardfault_increment_reboot(char *caller, bool reset)
 		syslog(LOG_INFO, "Failed to open Fault reboot count file [%s] (%d)\n", HARDFAULT_REBOOT_PATH, ret);
 
 	} else {
-
 		ret = OK;
 
 		if (!reset) {
@@ -1133,10 +1082,10 @@ __EXPORT int hardfault_increment_reboot(char *caller, bool reset)
 
 	return ret;
 }
+
 /****************************************************************************
  * hardfault_write
  ****************************************************************************/
-
 __EXPORT int hardfault_write(char *caller, int fd, int format, bool rearm)
 {
 	struct bbsramd_s desc;
@@ -1224,15 +1173,12 @@ __EXPORT int hardfault_log_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "check")) {
-
 		return hardfault_check_status(self);
 
 	} else if (!strcmp(argv[1], "rearm")) {
-
 		return hardfault_rearm(self);
 
 	} else if (!strcmp(argv[1], "fault")) {
-
 		int fault = 0;
 
 		if (argc > 2) {
@@ -1242,17 +1188,13 @@ __EXPORT int hardfault_log_main(int argc, char *argv[])
 		return genfault(fault);
 
 	} else if (!strcmp(argv[1], "commit")) {
-
 		return hardfault_commit(self);
 
 	} else if (!strcmp(argv[1], "count")) {
-
 		return hardfault_increment_reboot(self, false);
 
 	} else if (!strcmp(argv[1], "reset")) {
-
 		return hardfault_increment_reboot(self, true);
-
 	}
 
 	print_usage();

src/systemcmds/tests/test_bson.cpp

@@ -66,15 +66,15 @@ encode(bson_encoder_t encoder)
 		return 1;
 	}
 
-	if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0) {
+	if (bson_encoder_append_int32(encoder, "int1", sample_small_int) != 0) {
 		PX4_ERR("FAIL: encoder: append int failed");
 		return 1;
 	}
 
-	if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0) {
-		PX4_ERR("FAIL: encoder: append int failed");
-		return 1;
-	}
+	// if (bson_encoder_append_int64(encoder, "int2", sample_big_int) != 0) {
+	// 	PX4_ERR("FAIL: encoder: append int failed");
+	// 	return 1;
+	// }
 
 	if (bson_encoder_append_double(encoder, "double1", sample_double) != 0) {
 		PX4_ERR("FAIL: encoder: append double failed");